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Chapter 1 

MSFC FLAT FLOOR FACILITY 

1.1 INTRODUCTION 

This is the final report prepared for NASA George Marshall Space 
Flight Center (MSFC) by The University of Alabama in Huntsville (UAH) as 
part of the deliverables of a contract (NAS8-35670) awarded to UAH in 1984. 
The initial period of performance was eight months, and two subsequent 
modifications to the contracts were made. The entire task terminated in 
May 1985. The scope of work entails the development of software to drive 
the flat floor simulation facility at MSFC. 

At the conclusion of the contracted period, a final report was not 
submitted by UAH because it was not possible to verify the functionality of 
the control software. This was due to a series of hardware modifications 
of the facility. In January 1987, most of the hardware modifications and 
upgrades were completed, and the system was available for testing the soft- 
ware. The principal investigator, who, by that time had left UAH, worked 
with MSFC engineers at no cost to MSFC, conducted tests to demonstrated 
that the software was indeed working as expected. The mobile base was part 

i 

in a closed-loop control in January 1987. This explains the delay in sub- 
mitting the final report. 

1.2 THE ORBITAL MANEUVERING VEHICLE (OMV) 

The Orbital Maneuvering Vehicle (OMV) has been designed to operate as 
a remotely controlled space teleoperator. This vehicle will be deployed as 
a payload from the space shuttle. Control of the OMV will be from a ground 
station, or a control room located on the shuttle or the space station. 

The operator controlling the OMV is physically remote from the module and 



exercises control over the vehicle. The main mission of the OMV will be to 
increase the level of space productivity without increasing human risk. 

The OMV will not only reduce risk in orbital activities, but also increase 
the capacity to perform strenuous orbital operations. It will drastically 
reduce the level of EVA for a given mission. Unlike EVA, the OMV will not 
be affected by prolonged operational durations; also, it will be able to 
operate at ranges beyond EVA capabilities. The OMV has been designed to 
handle significant masses on the order of 45,000 pounds. The design should 
give the OMV the capability to: 

0 Deploy satellites in orbits that are out of the shuttle's range 
0 Rendezvous and dock with existing orbital payloads 
0 Resupply payloads with fuel and other consumables 
0 Perform repair and service operations on orbital payloads when 
fitted with a flight telerobotics system (FTS) 

0 Transfer payloads to or from orbit to the orbitting shuttle or 
space station. 

With these capabilities, the OMV will have a definite impact on the way 
orbital operations are carried out. Figure 1-1 shows an application over- 
view. To assemble an accurate simulator, the preliminary design of the 
actual OMV was studied. This design was reported in the Preliminary 
Definition Study of the Teleoperator Maneuvering System (TMS), prepared by 
program development at MSFC Cl]. This document is used to obtain critical 
specifications that are needed for simulator design. These specifications 
include the vehicle's size, shape, mass, docking mechanisms, and attitude 
control system. The preliminary design of the Orbital Maneuvering Vehicle 
is shown in Figures 1-2 through 1-4. The following is a list of key 
assumptions and guidelines for the MSFC reference design: 




Figure 1-1. Applications Overview 
















Figure 1-4. Exploded View 



0 Payload placement/retrieval capability 
0 Shuttle orbiter based with LEO/GEO mission capability 
° Minimum practical length and weight 
0 Minimum orbiter interfaces 

° Installation capability at multiple locations in cargo bay 
° Satisfaction of safety requirements of NASA 
° Monitoring and safing capability from orbiter AFD 
0 Potential for being space based at either LEO or GEO 
0 Control from ground station 

0 Capability to accommodate add-on kits and/or modifications for 
future extended capability and unique mission activities 
0 Maintain modularity to extent practical to accomodate hardware 
replacement 

0 On-orbit serviceability should be a design consideration 
0 Use of existing/developed hardware to extent practical 
0 Redundancy in critical areas 

° Degree of autonomy necessary to preclude continuous ground control 
0 Safe hold capability to survive a single failure 
0 Design for 10 year life with refurbishment. 

In order to verify these operational concept, a simulator was needed 
to permit extensive testing, modeling, and evaluation of the parameters 
involved in orbital operations. These tests include docking mechanisms, 
target motion, and human factors. Flexibility is of key importance in 
developing a simulation of his type. Ease in reconfiguring the simulator 
is essential. This reconfiguration may be through a series of hardware 
upgrades, such as additional degrees of freedom, propulsion system changes, 
front end assemblies, etc., or through software enhancement in the OMV 


mathematical model. 



1.3 FLAT FLOOR FACILITY 


The overall simulation system is shown in Figures 1-5 and 1-6. The 
three major subsystems are: 

(1) Control console equipped with hand controller and display units 

(2) Mainframe containing the OMV response model, orbital mechanics, 
and state vector transformation 

(3) Mobility vehicle (TOM-B) with the flat floor and dynamic target 
simulator 

Each of these subsystems have been further subdivided into modular com- 
ponents to give added flexibility. Detailed implementation will be given 
for each of the major subsystems. The overall control flow in block 
diagram form is given in Figure 1-7. The function and responsibilities of 
each subsystem can be summarized in the following: 

Control Room - The control room is to serve as the man-machine inter- 
face. This interface consists of a command station (hand 
controllers) and sensory feedback devices (video monitors, status 
screens, etc.). The commands are then sent to the mainframe sub- 
system. 

Mainframe Subsystem - The mainframe subsystem is to accept the hand 
controller commands, process these commands with respect to the OMV 
mathematical model, then generate and transmit the appropriate mobi- 
lity base commands. 

Mobility Base - The mobility base subsystem is to execute the 
generated commands from the mainframe. This consists of a number of 
vehicle movements to achieve the intent of the hand controller input. 


1.4 SUBSYSTEM DESCRIPTION 
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Figure 1-5. MSFC Flat Floor Simulation System 





















MSFC ROBOTICS FACILITY 
(FLAT FLOOR! 



Figure 1-6. MSFC Flat Floor Facility 










ARROWS SHOW COMMUNICATION FLOW 


Figure 1-7. Control Flow 





1.4.1 Control Room Subsystem 


The control room is the center of all teleoperation activities. This 
control room may be located on the ground, in the space shuttle or within 
the space station. The simulator control room is located adjacent to the 
flat floor, and its interior is depicted in Figure 1-8. From this loca- 
tion, an operator can control the simulator vehicle and cognitively sense, 
through various feedback methods, the overall operation. This is the idea 
of telepresence. The degree of telepresence is a function of the sensory 
feedback. This section will outline both the current and proposed types of 
sensory feedback. The degree of telepresence necessary for successful OMV 
man-machine interfacing is not well defined at this point in time. 

Critical human factors design is, however, beyond the scope of this work. 

The main feedback element is direct video from cameras mounted on the 
vehicle. The video feedback is displayed on the screens in front of the 
pilot, as shown in Figure 1-8. Each screen will give a different view 
relevant to the operation to be performed. As currently configured, this 
is the only sensory feedback available to the operator. Modifications to 
be made to the control room include adding a status screen so that the 
operator will have pertinent data such as range, range rate, fuel deple- 
tion, force/torque, etc. NASA is at present evaluating the use of 
stereoscopic vision systems and 3-D displays. This would allow the opera- 
tor to observe one main screen as opposed to correlating the views from 
several screens. Other modifications may include optical proximity sensing 
for collision detection, tracking, and centering operations. Touch 
screens, menu driven subsystems, and a mouse may be used. These feedback 
devices recreate a realistic scenario of the workspace within the control 
room. This technique will allow effective remote servicing capability. 
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Figure 1-8. Control Room 



The other major function of the control room subsystem is to accept 
operator control inputs. Control authority must be fast and efficient to 
permit teleoperation. The central input devices used are two 3 DOF hand 
controllers. 

In the present implementation two hand controllers are used. One is 
used to control the translational axes - X, Y, Z, while the other controls 
rotational motion - roll, pitch, and yaw. These hand controllers give the 
operator full control over the vehicle. Full detail of the hand controller 
hardware and software will be given later. 

The communication system that connects the control station to the OMV 
is a very critical component in the subsystem. This system defines the 
feedback and control limitations involved in teleoperation. Specifications 
for the OMV include communication via the Tracking and Data Relay Satellite 
System (TDRSS). All communication will be processed through this link. 
Because of the inherent time delay constraint involved when transmitting 
over large distances, NASA has chosen to incorporate this time delay into 
the OMV simulation. This will allow testing of variable time delays and 
their effects on the command and control that the operator will experience. 
The data rate limit for TDRSS is 1 Mbps down and 10 Kbps up, which requires 
that the standard video data rate be reduced to lower frame rates, lower 
pixel resolution, and adaptive encoding [2]. Figure 1-9 gives the overall 
communication data flow. 

1.4.2 Mainframe Subsystem 

The mainframe subsystem is responsible for accepting inputs from the 
control room subsystem and generating the appropriate commands to the mobi- 
lity base subsystem. The mainframe subsystem hardware is composed of a 
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Figure 1-9. Communication Data Flow 

















mainframe digital computer and the communication equipment. Connection 
between the mainframe subsystem and the mobility base subsystem is achieved 
via the communications network. The mainframe subsystem software consists 
of a module code named OMM, which is a mathematical model of the actual 
OMV. Each of the primary hardware and software components will be 
described in the subsequent chapters. 

The computer used to process all off-vehicle computations is Digital 
VAX 11/750 minicomputer. This computer will be central in processing and 
controlling the data flow between the control room and the mobility base. 
The VAX 11/750 will process the hand controller inputs and generate the 
appropriate commands to the mobility base. This computer is responsible 
for generating the major cycle interrupts. 

1.4.3 Mobility Base Subsystem 

The third major component of the OMV simulation is the mobility base 
subsystem. This subsystem receives commands from the mainframe subsystem 
via a telemetry link. The responsibility of the mobility base subsystem is 
to execute these commands. The mobility base subsystem contains both hard- 
ware and software components. The hardware includes the mobile base 
vehicle (code named TOM-B), the Orbital Maneuvering Vehicle mockup module, 
the flat floor, and the target motion simulator. A description of the 
mobile base (TOM-B) and its associated subsystems will be given in full 
detail. The software component of this subsystem consists of the on-board 
processing logic of TOM-B; it's design, implementation, and verification 
will also be given in complete detail. The flat floor on which the mobile 
base traverse measures 86 feet by 44 feet and is shown in Figure 1-10. The 
floor was constructed in 1982 to test vehicles with air bearings. It is 
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Figure 1-10. Flat Floor 



within .001 inch between any adjacent square foot and has an overall flat- 
ness of .032 inches in the plane. It has a reinforced concrete foundation 
with a special epoxy resin surface for low friction. 

The Orbital Maneuvering Vehicle mockup is shown in Figure 1-11. This 
mockup module was constructed according to the actual Orbital Maneuvering 
Vehicle specifications!!!]. This mockup was mounted on the front of the 
mobile base. Figure 1-12 shows this arrangement. This arrangement facili- 
tates realistic simulation of hardware-related operations such as docking, 
camera placement, etc. 

A target motion simulator was constructed to replicate the motion of 
an orbiting target. Since the Orbital Maneuvering Vehicle will have many 
diverse tasks, a general purpose target was constructed, that is, the 
target was constructed with a standard docking mechanism mounted on its 
front. The target is mounted on the end of a robot arm. This robot arm, 
built by Kadar Corp., has a 20 foot reach with a 1000 pound payload capabi- 
lity. With appropriate software, this robot can emulate spin and pre- 
cession motions which are common in orbiting satellites. The target motion 
simulator is mentioned because it is part of the overall Orbital 
Maneuvering Vehicle simulation, and will be used in testing. The detailed 
design and implementation is beyond the scope of this paper and will not be 
presented here. This robot arm, unique because of its size and performance 
specifications, is shown along with the mounted target in Figure 1-13. The 
mobility base has been code named TOM-B and will be referenced as such. 
TOM-B is a vehicle with air bearings that floats on the flat floor. The 
vehicle has six degrees of freedom. The vehicle is capable of transla- 
tional and rotational motion. The X and Y translational and yaw motion is 
accomplished through the air bearing pads. The Z axis is driven by a DC 
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Figure 1-11. TOM-B Simulating Docking 
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Figure 1-12. TOM-B with Docking Mechanism 
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Figure 1-13. Target Motion Simulator 



drive motor and associated gear train. Similarly, the rotational motion of 
pitch and roll is fulfilled by DC drive motors and gear trains. X and Y 
translation is confined by the dimensions of the flat floor, which is 96 
feet diagonally. Z motion is restricted to plus or minus 20 inches from 
the center of the drive train. Pitch is limited to plus or minus 20 
degrees referenced from the horizontal center line. The other rotational 
axes, yaw and roll, are continuous. By executing appropriate motions, 
realistic Orbital Maneuvering Vehicle motions can be achieved. Note that 
the commands received from the mainframe subsystem emulate orbital motion. 
Thus, the motion of TOM-B is not necessarily that of the mockup module. 

For example, if the mockup module were to execute a yaw about it's Z axis, 
the output of the mainframe subsystem would generate a sequence of commands 
to TOM-B to execute a translation plus a rotation. The characteristics of 
TOM-B are shown in Table 1-1. 


TABLE 1-1 

Approximate mass: 

Moment of Inertia: 

Mass of fuel : 

Number of thrusters: 

Thrust developed: 


1360.5 kg 

100 kg-m^ 

136.4 kg 
24 

13.2 Newtons/thrust 


Two sets of three thrusters, each of which is capable of delivering 13.2 
Newtons of force, are mounted on each corner of the vehicle. Cold 
compressed air at 3500 psi is used as propellent. Six propellents tanks 
are used, four of which are used for thruster firings and two for the air 
bearing pads, as shown in Figure 1-14. Note that the thrusters are non- 
throttable. The translation in the X and Y axes, as well as yaw motion of 


mounting 
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Figure 1-14. TOM-B Showing the Six Degrees of 
Freedom & Thruster Assembly 




the vehicle, are obtained by firing the appropriate thrusters. Translation 
along the Z axis, as well as pitch and roll are carried out using stepping 
motors fitted with resolvers. The overall hardware organization is given 
in Figure 1-15. 

The computer and associated electronics are mounted on the rear of 
the vehicle. The control electronics include the A/D, D/A, modem, and sen- 
sor processing boards. The vehicle is fitted with X and Y accelerometers. 
To give orientation feedback a gyroscope is used. In the initial hardware 
configuration the accelerometers are used for measuring velocity and posi- 
tion, and the gyro is used to measure orientation of the vehicle. The gyro 
installed on TOM-B has a total error rate of 5 x 10"6 degree/sec, which is 
more than adequate to provide feedback information on angular velocity and 
displacement. The accuracy is not present with the accelerometers[27] . 

The large error arises from the facts that: 

1) The sensor has a high drift rate. 

2) The signals from the sensors must be integrated numerically to 
obtain the translational displacement. 

3) The errors are cumulative and propagate with time. 

The accelerometers and gyro are actually designed to measure accelerations 
and angular velocities, respectively. When the signals must be integrated 
to get displacement, the following steps must be carried out: 

1) They must be sampled frequently within every major cycle. 

2) The signals must be conditioned and corrected for bias, scaling, 
offset, and drift. 

3) To provide reliable displacement, sophisticated integration 
algorithms must be used. 
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Figure 1-15. TOM-B Control Hardware Organization 











All these factors contribute to large computational overhead. In addition, 
there is no meaningful method for correcting the drift, other than using 
some external reference scheme. 

Since position control is used in the present system, it is mandatory 
to have an accurate navigation system. This effectively rules out the use 
of accelerometers to provide positional feedback. An alternative, simpler 
navigation system is needed. This new navigation system does not replace 
the accelerometers; they are still needed to provide the rate feedback. 

The navigation system works on the principle that reflectors are mounted 
around the perimeter of the floor. A positionable distance meter, mounted 
on the vehicle, detects these reflectors. The system has two of these 
devices mounted on the front of the vehicle. It is estimated that a posi- 
tional accuracy of several millimeters can easily achieve in this way. 

More importantly, the computation is relatively straightforward, fast, and 
the error does not propagate with time. This navigation system provides 
position feedback necessary for control of the vehicle. A detailed 
discussion of the design and implementation of the navigation system will 
not be presented here. 

1.5 SOFTWARE DESCRIPTION 

The current task as mentioned primarily is to develop suitable soft- 
ware as part of the flat floor simulation system so that it can be used to 
realistically study the behavior of the OMV. The software is made up of 
three major modules: a) the OMV mathematical model (OMM) which accepts 

operator input from the control station and compute the state of the OMV, 
b) the State Vector Transformation Module (SVX) which translates the OMV 
state vector into a set of commands for the mobility base, and c) mobility 


base control logic TOM-B. When these commands are executed, the mobility 
base would have moved in such a manner that the OMV mockup mounted on it 
would have replicated the motion of the OMV. Figure 1-16 depicts the con- 
nectivity of these components. 

Chronologically, SVX was developed first, followed by TOM-B, and OMM 
was developed last. However, OMM and SVX was tested and verified first, as 
these two modules are hardware independent, while TOM-B was test verified 
last. For the purpose of this report, the OMV mathematical model OMM will 
be describes in Chapter 2, the State Vector Transformation module SVX will 
be described in Chapter 3, and TOM-8 in Chapter 4. A summary of testing 
procedures and conclusions will be presented in Chapter 5, together with 
the test date obtained. 



Figure 1-16. Flat Floor Facilities — Software Architecture 











Chapter 2 

OMV Mathematical Model (OMM) 

2.1 INTRODUCTION 

This report discusses the design and implementation of OMM - a mathe- 
matical model of the Orbital Maneuvering Vehicle [3]. The Orbital 
Maneuvering Vehicle (OMV) can be maneuvered by remote operator control. 

Its motion is completely specified by its equations of motion. The solu- 
tion of the equations of motion yields its position [X,Y,Z] T , velocity 
[X,Y,Z] T , orientation [r,p,y]T and their rates [r,p,y]T where r, p and y 
stand for roll, pitch and yaw respectively. From these dynamic quantities, 
a 14-component state vector can be generated. This state vector contains 
all the necessary information to completely specify the state of the 
vehicle in space at any time. 

The OMM simulates the motion of the Orbital Maneuvering Vehicle in 
space. OMM is a software subsystem that is an integral part of the soft- 
ware system used to drive the MSFC flat floor simulation system. In this 
installation, a set of hand controllers is used to maneuver the OMM 
(Mathematical model) and the state vector obtained is used as input to a 
second software module called SVX (the State Vector Transformation module) 
which transforms it to a suitable set of commands to be transmitted to, and 
thereby controlling the motion of the mobile base on the flat floor. The 
over-all relation is as shown in Figure 2-1 as can be seen in this figure, 
the OMV module encompasses the vehicle response module as well as the orbi- 
tal mechanics module. In order to optimize execution speed, these two 
modules are not implemented as separate entities. 

The State Vector Transformation Module will be discussed in the next 
chapter. Throughout this report, it is important to bear in mind that the 
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Figure 2-1. MSFC Flatfloor Simulation System 








OMM simulates the motion of the Orbital Maneuvering vehicle but otherwise 
has no physical relationship with the Orbital Maneuvering Vehicle. The 
mobility base on the flat floor will attempt to move in such a manner that 
a mockup module mounted on it will replicate the motion of the Orbital 
Maneuvering Vehicle, using a set of commands derived from the state vectors 
generated by OMM. Otherwise the mobile base is not related to the OMV. 
the mockup module is not the Orbital Maneuvering Vehicle. One of the 
objectives of the flat floor system is to simulate docking of the OMV with 
a target vehicle [4]. 

2.2 THE OMV MOOEL 

This section describes a simplified mathematical model of the Orbital 
Maneuvering Vehicle. A more detailed model is being developed elsewhere at 
MSFC. In the present model, several simplifications and assumptions have 
been made. The objective is to develop quickly (and hence the simplifica- 
tion) a model that can be used to drive the flat floor system. 

Before discussing the model in any detail, it is necessary to define 
the various coordinate systems used in this work. 

A. The Local Vertical Frame (LVF) 

Imagine a space craft in an orbit around the earth. It is immaterial 
whether this is the Orbital Maneuvering Vehicle or the target vehicle. LVF 
is a coordinate system with its origin at the center of mass of this space 
craft such that Z-axis lies in the plane of the orbit and is directed away 
from the center of the earth. The Y-axis is chosen to be parallel to the 
orbital angular momentum vector and X-axis is tangential to the orbit as 
shown in Figure 2-2. The position, velocity as well as orientation of the 
second vehicle are described in LVF and is therefore relative to the 



orbiting vehicle. Throughout this work, it is assumed that the target 
vehicle is the orbiting vehicle. 

B. OMV Body Frame 

This is a body fixed reference frame with its origin fixed at the 
center of mass of the OMV, and its axes will be denoted by 1, 2 and 3 
respectively. Initially, at the start of the simulation, 1, 2 and 3 axes 
line up with X, Y and Z axes respectively. As can be seen from Figure 2-3, 
the axis of symmetry is the 1-axis. 

In order to construct the model of the Orbital Maneuvering Vehicle, 
the following assumptions are made: 

1. The OMV is assumed to be a circular disk of constant mass and 
having a uniform mass distribution. This assumption may seem 
unreasonable at first glance, but one quickly realizes that the 
detail shape of the OMV is unimportant as long as one knows the 
mass and propulsion characteristics of the Orbital Maneuvering 
Vehicle. In the present model, the mass characteristics are sum- 
marized in Table 2-1. These figures are taken from the MSFC 
Preliminary Definition Studies. 

2. The OMV is manipulated using signals from a set of hand 
controllers [5]. These signal can be classified into two groups. 
The first group is used to simulate a force acting through the 
center of mass of the OMV. In other words, one can, from this 
group of signals, generate an acceleration vector a = Cai,a 2 ,a 3 ] T 
in the body frame. The other group of signals simulates rota- 
tions about 1, 2 and 3 axes, namely, a vector w = [wi,w2,w3]T. 
Assumptions 1 and 2 mean that detailed knowledge of the shape. 




Dynamic Variable 

Value 

unit 

Mass M 

3282.75 

kg 

X 11 

7048.37 

kg m 2 

i 22 

3713.95 

kg m 2 

Z 33 

3713.95 

kg m 2 


Table 2-1. OMV Mass Characteristics 


thrust level and placement of the thruster and so forth are not 
really needed. The present control mode is the only mode imple- 
mented. 

3. Circular orbits are assumed. The altitude of the orbit can be 
anything from 150 to 1500 nautical miles which is within the 
designed operating range of the Orbital Maneuvering Vehicle. 

4. Orbital mechanics is an important part in describing the motion 
of the OMV and is therefore implemented. Other secondary pertur- 
bation effects are totally ignored. 

5. The state of the OMV is computed and updated 10 times per second. 
The period of 0.1 second will be referred to as a major cycle 
throughout this report. 

The equations of motion of the OMV can be discussed in terms of the 
rotational part and translational part. 

2.3 ROTATIONAL EQUATIONS OF MOTION 

The rotational equation of motion can be written as: 

T * L 


where L * Iw is the angular momentum vector and t is the applied torque. I 
is the moment of inertia tensor and w is the body rate. The solution can 
be drastically simplified by choosing the body axes 1, 2 and 3 such that I 
is diagonal [6,7], that is: 


I 


In 0 0 

0 1 22 0 

0 i 33 


0 


Remember that w = [wi,W 2,W3]T is obtained from the hand controller 

signals. The solution of the rotational equations of motion yields <fi , 0 
and <p the three Euler angles. The order and sense of rotation is chosen in 
the conventional manner [8], that is: 

t ♦Ilf® ]3[ i|>] 2 

To reduce computational overhead, quaternions are used to specify the atti- 
tude of the OMV rather than the Euler angles themselves. It has been 
proven that the two representatives are exactly equivalent [9]. A quater- 
nion q may be written as: 

q = iqi + jq2 + kq3 + q4 = Cqi^,^.^ 1 
and satisfies the relation 

2222 . 

q 1 + q 2 + 93 + 94 " 1 

An object whose attitude is described by the three Euler angles relative to 
some reference frame can be treated as a single rotation by a about an 
Euler axis E = [Ei,E2,E3]T. Theory has shown that this is the shortest 
angular path[loJin the sense that a is less than the algebraic sum of <f> # 0 
and ip . The angle a and the Euler axis can be expressed in terms of the 
quaternion q as: 

a 

cos -J = q 4 



E 


( i q i + jq 2 + *<13) / (<ii 


U 

2 


+ q 2 + q 3 ) 


Since the attitude control system of the OMV can control the roll, pitch 
and yaw axis independently, we expect the roll, pitch and yaw [r,p,y]T to 
be proportional to the respective components of E [10]. In fact, the 
following relation holds: 


[r,p,y] T = [ a E x , aE y , aEj 1 


Quaternion algebra leads to further computational economy when successive 
rotations need to be calculated. Let say, at any instant, the attitude of 
the OMV is specified by the quaternion q^ relative to some non-rotating 
frame . Suppose further that an instant later, the vehicle's attitude has 
changed, having rotated by $ e and ^ . These angular displacements are 
measured relative to the rotated body frame. If the new attitude is 
described by a second quaternion q 2 , the attitude of the vehicle, rel ati ve 
to the non-rotating frame [ 11 , 12 ] is then given by 

q = qid2 

This is an important advantage because if at the beginning of the simula- 
tion, the body frame is aligned with the LVF (as specified by the quater- 
nion qo = [0,0,0,1] T ), then the attitude of the OMV relative to the LVF, 
after n successive rotations is simply: 

q ■ qoqiq2---qn 

Of course, the attitude of the vehicle after the n+l-th rotation is q = 
q n q n +l* Thus, the attitude of the vehicle can be computed from the pre- 



vious quaternions. This recursive property gives rise to quite a com- 
putational advantage, especially since there are only four elements in a 
given quaternion versus the nine elements of a direction cosine matrix. 

2.4 EQUATIONS OF MOTION 

The translational equations of motion [ 8 ] has been derived in detail 
in Appendix I, and will not be repeated here. In essence, we seek solu- 
tions to a set of three simultaneous, coupled second order differential 
equations of the form: 

X - A x - 2 u Z 

•« 2 
Y = A y - w Y 

I! • 2 

l * A 2 + 2 oi X + 3 u> Z 


Here, the position and velocity vectors [X,V,Z] T and [X,Y,Z] T refer to the 
position and velocity of the OMV relative to the target vehicle, as 
expressed in Local Vertical Frame. u» is the orbital velocity, and A = 
CA x ,Ay,A z ]T is the linear acceleration vector in LVF. Remember that the 
hand controller signals give rise to an acceleration vector a = Cai,a2,a3] T 
in OMV body frame. Thus, one can obtain A from a using the transformation: 

A = C -1 a 


where C “1 is the inverse of the direction cosine matrix which can be 
derived from the quaternion q = Cqi,a2,a3,a4] T as: 


c - 1 


q 4 + qi - q2 - q3 2(qiq2 - q3<l 4 ) 2 (qiq 3 + q2q 4 ) 

2(qiq2 + Q3Q4) q 4 - Ql + Q2 - q3 2(q2q3 - qiq 4 ) 
2(qiq3 - q2<l 4 ) 2(q2q3 +qiq4) q4 - qi - q2 + q3 



It is obviously impractical to seek an analytical solution to the 
translational equations of motion. Numerical methods must be used. In the 
present work, the Adam-Bashforth method is used. For this purpose, each 
major cycle is subdivided into N (normally 10, but see later section) sub- 
intervals, each of which will be referred to as a minor cycle. It is 
necessary that the acceleration vector A be computed for each minor cycle, 
and stored in an acceleration matrix. At the end of N minor cycles, this 
acceleration matrix is used to obtain the numerical solution for the entire 
major cycle. A 14-component state vector is then assembled, and their com- 
ponents are listed below: 

S(l) - S ( 3 ) — relative position vector in LVF 

S(4) - S(6) — relative velocity vector in LVF 

S ( 7 ) - S(9) — angular momentum vector in LVF 

S ( 10 ) - S ( 13 ) — attitude quaternion 
S( 14 ) — mass in kilograms 

The angular momentum vector in LVF can be deduced as follows. Since the 
body rate w = [wi,W 2 ,w 3 ] t is known, one can calculate Lg in body frame 
using the relation 


Lb = Iw 

L = C-1l B 

where is the inverse of the direction cosine matrix. 

The state vector serves as input to the State Vector Transformation 
module (SVX). This module has been designed and implemented and will be 
described in Chapter 3. 


2.5 SYSTEM DESIGN AND IMPLEMENTATION 


The design and implementation of the present system is best discussed 
in the following sub-sections: 

A. Hand Controllers 

The hand controllers allow the operator to manipulate the Orbital 
Maneuvering Vehicle in terms of translation and attitude. In the present 
system, hand controller signals are used to maneuver the OMV model. The 
hardware is configured to provide 12 bits of information. The first 6 bits 
pertain to translation, while the remaining 6 bits pertain to attitude 
control. During development, the 12 bits are simulated by reading them 
from a disk file (HNDSGL.DAT) as 12 single digit integers. This process is 
carried out in a subprogram called HNDCTL. In actual implementation, this 
subprogram must be replaced by a suitable device driver. 

The bit assignment is shown in Table 2-2. It will be noted that 1 
will be used to denote the "on" state while 0 will be used to denote the 
"off" state. The subroutine HNDCTL contains sufficient logic to ensure 
that when both bits assigned to a given axis are on, they will be treated 
as both off (that is, no acceleration along, or rotation about, that axis) 
to conserve fuel usage. The main purpose of this subroutine is to examine 
the 12 bits from the hand controllers and return two vectors a and w where 

a = Cai,a 2 ,a 3 ] T and w = Cwi,w 2 ,w 3 ] t 

whose meaning have been explained in the previous section. It is important 
to remember that both a and w are expressed in the OMV body frame. 

Ideally, the hand controllers signals should be sensed and updated 
every minor cycle. But because of timing considerations they will be 
sensed once every major cycle, and it is explicitly assumed that the bit 



bit 


Meaning 


1 Acceleration along +1 direction 

2 Acceleration along -1 direction 

3 Acceleration along +2 direction 

4 Acceleration along -2 direction 

5 Acceleration along +3 direction 

6 Acceleration along -3 direction 


7 

+ roll; 

CCW 

rotation 

about 

1-axis 

S 

- roll; 

CW 

rotation 

about 

1-axis 

o 

y 

+ pitch; 

CCW 

rotation 

about 

2-axis 

10 

- pitch; 

CW 

rotation 

about 

2-axis 

n 

+■ yaw; 

CCW 

rotation 

about 

3-axis 

12 

- yaw; 

CW 

rotation 

about 

3-axis 


Table 2-2. 


Hand Controller Bit Assignments 



states do not change during the entire major cycle. This is not an 
unreasonable assumption, since one major cycle is 0.1 second, which is in 
the neighborhood of the average reaction time of the human operator. 
Besides, the OMV does not have a fast response because of its large mass 
and low thrust levels. 

The acceleration vector a must be expressed in LVF before it can be 
used in solving the equations of motion. In the OMV software, this is 
carried out as mentioned previously by: 

a) Calculating the inverse of the direction cosine matrix C“l, 

b) Transforming the vector a to A in LVF, and 

c) Placing A in an acceleration matrix AA. 

Step a) is carried out by a subroutine called DCSINV while steps b) and c) 
are carried out by subroutines DMUL and STORE in subroutine MOTION. At the 
end of the N minor cycles, the subroutine SOLVE is invoked to obtain solu- 
tions to the equations of motion numerically. 

B) Numerical Solutions: 

A three step Adam-Bashforth method [15] is used to obtain solutions 
to the equations of motion. This method is well known, and will not be 
elaborated here. Essentially, the set of three coupled second differential 
equations are re-written as a set of six simultaneous first order differen- 
tial equations, and the solution computed. The six initial conditions 
needed for the computation are provided by the six components of the rela- 
tive position and velocity vectors. Subroutine SOLVE takes the relative 
displacement and velocity vectors as initial conditions of the previous 
major cycle, and returns the new positions and velocity vectors. A 
subroutine called STATE is then invoked to assemble the state vector. 


C) Output Section: 

A subroutine called OUTPUT is responsible for conveying information 
to the outside world. In normal operations, no output is generally 
expected, but during testing, it is necessary to be able to monitor the 
progress of the simulation. At present, one can, via the use of flags, 
control the form and type of output. By way of example, one can request 
OMV to print a time sequence of state vectors at 1 second intervals on the 
printer, or display the position and orientation of the mobile base (on the 
flat floor) graphically, or disable all outputs altogether. 

A fairly simple graphics package called PLOT is implemented to pro- 
vide graphics output. This package is developed for the initial software 
checking only; namely to provide to operator with some form of visual out- 
put and is not construed as a deliverable. It must be emphasized that this 
package is hardware dependent, and is not compatable with the POP 11/34 
mini -computer. The present graphics package runs on an IBM Personal 
Computer fitted with a TECMAR GRAPHICS MASTER board and an IBM monochrome 
monitor. A resolution of 640 by 352 is used for the package, although the 
system has a potential resolution of 720 by 700 pixels [16]. PLOT uses 
escape codes to generate the top or side view of the mobile base (including 
the mock up module). A listing of this package, written in FORTRAN 77, is 
included in Appendix 2. It is anticipated that this package can be 
modified to run on the Evans and Sutherland color graphics terminal driven 
by a VAX 780. 

The entire OMV module is written in FORTRAN 77, and all floating 
point computations are carried out in double precision. The usual struc- 
tured programming technique is used [14]. Modular design is faithfully 
adhered to, so that subroutines can be easily updated or replaced. At 


times, efficiency may be sacrificed for code clarity, thereby making the 
code much easier to maintain and modify. During the design phase, flexibi- 
lity is emphasized. Model parameters are inputted from disk files. Thus, 
modifications on the flat floor system will not involve any changes to the 
OMV source code. Appendix 3 shows the various data files used. 

Explanations for the various quantities are included as part of the record 
so that one can easily modify the configuration, initial conditions and so 
forth without having to refer to the source listing. A complete listing of 
OMV is included in Appendix 4, and a hierarchal chart is shown in Figure 
2-4. 


2.6 TESTING AND RESULTS 

Initial testing of the OMV software is conducted using an IBM 
Personal Computer with 8087 arithmetic co-processor. The same source code 
without the graphics option has been uploaded to the POP 11/34 and VAX 750 
at MSFC and executed successfully. 

The nature of the model is such that the major source of error would 
arise from the numerical solutions of the equations of motion. Thus, much 
effort has been spent to ensure that the Adam-Bashforth method yields 
accurate results. An error analysis of this method shows that the error is 
of the order of h$ where h is the step size. In the present work, the step 
size is typically 0.01. This, coupled with the fact that all computations 
are carried out in double precision, means that the expected truncation 
error is of the order of 10"10 — a figure that is too good to be true. 

The following tests were conducted to verify that this method does 
indeed give accurate solutions. The homogeneous case is first considered. 
Physically, this corresponds to the situation where the operator leaves all 
the controls in neutral so that 


Figure 2-4. OMV Heirarchial Chart 



PLOT (Note 1) 
(Note 1) 
(Note 1) 

FUDGE 

DOTPRD 

PUT 

DCSINV 

DMUL 

MATCH 

UPDQ 

DETQ SINCOS 

STORE 



ANGFRE 

VECTOR 

(Note 2) 


Note 1 : Hardware incompatible graphics package. 


Note 2 : 


Vector Transformation Module. See Reference 1. 


a = [0,0, 0]^ and 


w = [0,0, 0]T 


Thus, the equations of motion reduce to: 

• • * 

X = - 2 u Z 

2 

Y * - w Y 

Z 2 “ X + 3 o) 2 Z 

This set of equations can be solved numerically using the Adam-Bashforth 
method. Further, if Xi, X2, X3 and Vi, V2, V3 are the initial conditions, 
it can be shown that the analytical solutions are: 

x(t) . X t - (3at - fl 4sinn H . v t - 6(nt - sinnt) x 3 - v 3 

X(t) = - (3 - 4 cosat) Vj - 6n( 1 -cosnt) x 3 - 2 (sinat) v 3 

T(t) = (coset) X^ + 

Y(t) = -n(sinat) X ? + (cosnt) V 2 

Z(t) = 2(1 ~ c ° SQt) v 1 + ( 4 - 3 cosnt) X 3 + V 3 

Z(t) = ?( sinnt) v 3 + 3n(sinnt) x 3 + (cosnt) v 3 

Thus, the numerical solutions can be compared directly with the analytical 
ones. Here, n is the orbital velocity, and for a circular orbit, a can be 
calculated: 


n - G M e / (R 0 + H) 3 


where G is the universal gravitation constant, M e is the mass of the earth, 
R 0 is the mean earth radius and H is the altitude. Note that at higher 
orbits, fi approaches 0 and the equations of motion approach 

X -> 0 

Y -> 0 

Z -> 0 

and better agreement between numerical and analytical results are expected 
for high altitudes than lower orbits. A computer program called ADAM has 
been developed that would, given a set of initial conditions, calculate 
both the numerical and analytical solutions to the equations of motion. 

The source listing of ADAM is shown in Appendix 5. In the present set of 
tests, an altitude of 200 kilometers (n = 0.00118 rad/sec) is used 
throughout. This altitude represents the lowest design orbit of the 
Orbital Maneuvering Vehicle. Table 2-3 shows a comparison between the ana- 
lytical and numerical solutions at this altitude, using the initial con- 
ditions: 


Xi = 0, X 2 = X 3 = 0 
Vi = 0.05, V 2 = V 3=0 

The results shows that the two solutions agree to better than 3 x 10“8 in 
60 minutes, or about 0.03 millimeters. This figure is well below the 
expected accuracy of the flat floor simulation system. This suprisingly 
small error comes from the fact that the angular velocity n is quite small. 
When ft = 1.0 is used, (this angular frequency does not make sense physi- 
cally, as it represents an orbit well below the earth's surface, but 



Time 

in 

Minutes 

X (meters) 

Z (meters) 

Numerical 

Analytical 

Numerical 

Analytical 

0 

0.000000 

0.000000 

0.000000 

0.000000 

5 

13.746736 

13.746736 

5.271240 

5.271240 

10 

20.161917 

20.161917 

20.427114 

20.427114 

15 

12.828963 

12.828962 

43.576117 

43.576178 

20 

-12.952950 

-12.952952 

71.829442 

71.829444 

25 

-59.582227 

-59.582233 

101.660919 

101.660923 

30 

-126.855533 

-126.855544 

129.347660 

1 

129.347664 

35 

-211.993176 

-211.993191 

151.434377 

151.434380 

40 

-309.986003 

-309.986022 

165.164663 

165.164664 

45 

-414.220544 

-414.220565 

168.824984 

168.824985 

50 

-517.304365 

-517.304388 

161.958539 

161.958536 

55 

-611.988644 

-611.988666 

145.422253 

145.422248 

60 

-692.072815 

-692.072834 

121.279843 

121.279843 


Note : X and Z are expressed in Local Vertical Frame. 


Table 2-3. Comparison Between Analytical and Numerical Solutions 



constitutes a valid situation mathematically), the errors propagate quite 
fast as to render the comparison meaningless after 10 minutes. 

A second test was carried out at the same altitude, using null ini- 
tial conditions: 


Xi * X 2 = X 3 = 0 
Vi = V 2 » V 3 = 0 

The hand controller signals were chosen to yield a constant acceleration 
along the X-axis in the LVF, that is a = [0.025,0,0]^, and the orientation 
of the OMV is chosen to be aligned to the LVF at t = 0. The result after 4 
seconds of simulation is shown in Table 2-4. A plot of the relevant dyna- 
mic variables as a function of time is shown in Figure 2-5. The result 
shows that the model behaves exactly as expected; namely that an accelera- 
tion along the X-axis gives rise to a Z component, as dictated by orbital 
mechanics. If we ignore the Z contribution for the time being, one can 
estimate the value of X and X using Newton's laws (this is not an invalid 
estimate as the time interval is quite short compared with the period of 
rotation) to be X = 0.2 meters, and X = 0.1 meter/sec respectively. These 
figures compare very favorably with the numerical results at t = 4 seconds. 

A very interesting test was conducted in which the OMV is made to 
execute a pure pitch motion. In this test, it is assumed that the OMV is 
originally at rest, the initial conditions being: 


Xl - X 2 * X 3 = 0 


Vi = V 2 = V 3 = 0 
r = p = y = 0 


where r, p, y represent the roll, pitch and yaw respectively. A pure pitch 
motion would correspond to a rotation about the 2-axis. Mathematically, 



— 

1 

X 

in 

meters 

2 

in 

meters 

Z 

in 

meters 

.0 

0.000000 

0.000000 

0.000000 

0.000000 

.5 

0.002940 

0.012125 

0.000001 

0.000007 

1.0 

0.012128 

0.024625 

0.000009 

0.000029 

1.5 

0.027565 

0.037125 

0.000032 

0.000065 

2.0 

0.049253 

0.049625 

0.000077 

0.000117 

2.5 

0.077190 

0.062125 

0.000152 

0.000183 

3.0 

0.111377 

0.074624 

0.000263 

0.000264 

3.5 

0.151814 

0.087124 

0.000418 

0.000360 

4.0 

0.198501 

0.099624 

0.000625 

0.000471 


Initial conditions : 

Xj = X 2 * X 3 = 0 and 

V 1 = V 2 55 V 3 * 0 

Note : All quantities are expressed in Local Vertical Frame. 


Table 2-4. OMV Acceleration Along +X Direction 








IN METERS 



TIME (SEC) 


Figure 2-5. Translation Along X-Axis 


IN 10' 4 METERS 




r = y = 0, and p = W2 = 0 


When the OMV is executed in this mode, the state vectors are fed into the 
SVX module, with the result that the state vector is translated into a 
sequence of commands CMD. This sequence of commands is to be transmitted 
to the flat floor. Table 2-5 shows the relevant commands for the mobile 
base. As verified by the graphics display, the mock up module mounted on 
the mobile base executes a pure pitch at the same rate as the OMV, while 
the mobile base has to translate along the +X direction. In addition, the 
pivot point is progressi vely lowered as expected. This test shows that the 
modules OMV and SVX are properly interfaced, and that correct results are 
produced. The command strings as outputted by the system to the flat floor 
is shown in Figure 2-6. 

To further ascertain that the system is functioning properly, the 
hand controller signals corresponding to a translation along 1-axis and a 
yaw is generated. The relevant commands to the flat floor system is shown 
in Table 2-6. A pictorial representation of the mobile base and mock up is 
as shown in Figure 2-7. Note that the path of the center of mass of the 
mock up exactly duplicates that of the OMV. 

In summary, various tests have shown that the OMV-SVX system func- 
tions properly. By way of example, a pure yaw motion of the OMV demands 
that the mobile base describes a circular path as shown in Figure 2-8. 

There is just one area that needs further investigation, namely timing con- 
siderations. This system must be able to complete all the computation 
within 0.1 second — a major cycle. When the system is uploaded to the PDP 
11/34, it was discovered that the computer took more than 0.1 seconds to 
complete one major cycle of computation. At this juncture, one can take 



Time 

Pitch 

X 

Z 

(Sec) 

(Rad) 

(meters) 

(meters) 

0 



2.4384 

4 

0.0698 

5.0010 

2.3852 

8 

0.1396 

5.0074 

2.3324 

12 

0.2094 

5.0167 

2.2800 

16 

0.2793 

5.0295 

2.2284 

20 

0.3491 

5.0460 

2.1778 

24 

0.4189 

5.0659 

2.1285 


Note : All measurements are in flat floor coordinates. 

Please see Appendix 1. 


Table 2-5. 0MV--Pure Pitch Motion at 0.017453 rad/sec 










xmasmm 

1 

X 

(meters) 

Y 

(meters) 

Z 

(meters) 

Yaw 

(rad) 

0 

0.0000 

11.6680 

2.4384 

0.0000 

4 

0.2752 

11.2418 

2.4390 

0.3470 

8 

1.0709 

11.0039 

2.4433 

0.6940 

12 

2.2919 

11.1199 

2.4545 

1.0410 

16 

3.7925 

11.7135 

2.4750 

1.3880 

j 

20 

5.3934 

12.8512 

! 

2.5062 

1.7350 

24 

6.9035 

14.5350 

2 . 5480 

2.0820 


Table 2-6. Motion of the Mobile Base Under Constant Acceleration 
of (0.025,0,0)' and Constant Yaw at 0.08675 rad/sec 




IN METERS 




Y IN METERS 



Figure 2-8. OMV Pure Yaw Motion 




one of the following three corrective actions: 

a) Use a faster host computer (VAX 780) 

b) Use single precision computation, or 

c) Increase the step size in the numerical methods. 

Of the three choices, the first method is clearly desirable, but until the 
VAX is installed, one must explore the remaining alternatives. Table 2-7 
shows a time comparison between single and double precision arithmetic when 
the OMV is run until identical parameters on the PDP 11/34 computer. The 
result shows little improvement in execution time. This is not surprising 
since the computer is equipped with hardware floating point capability. 

The only remaining recourse is to increase the step size, thereby reducing 
the number of steps (and hence the number of iterations). It is discovered 
that the numerical solution to the equations of motion [13] took most of 
the computation time. Table 2-8 shows a similar time test for various 
steps N and retaining double precision arithmetic after the code has been 
suitably optimized. The data show that a step size of h = 0.025 seconds (N 
= 4) satisfies the time requirement. The price to be paid is that the 
error associated with the numerical process may increase. Table 2-9 shows 
a comparison test for N = 10 and N = 4 using the program ADAM. The result 
suggests that there is an optimum N somewhere between 4 and 10 in which the 
error is a minimum, but this question is not pursued any further. The 
result also shows that the error does not increase substantially over the 
same period of 60 minutes whether we use N = 10 or N = 4. Using N = 4, the 
deviation from the analytical solution is still much less than the accuracy 
of the flat floor system. 

The series of tests conducted, some of which are not reported here, 
shows that the simplified mathematical of the Orbital Maneuvering Vehicle 



Mo of 

Average execution time per major cycle 

Steps 

Single Precision 

assssssssssssssssssas: 

Double Precision 

sasassasasasssasssaEassasassssassas 

4 

0.077 

0.084 

5 

0.090 

0.099 

6 

0.103 

0.113 

7 

0.117 

0.128 

8 

0.130 

0.143 

9 

0.144 

0.158 

10 

0.157 

0.173 


Table 2-7. OMV Time Test 



N 

Execution time (Sec) 

4 

0.068 

5 

0.079 

6 

0.090 

7 

0.100 

8 

0.111 

9 

0.122 

10 

0.132 


Table 2-8 


Optimized OMV Execution Times Per Major Cycle 
as a Function of Number of Steps N 



Time 

Solution 

in 

Analytic 

Num 

eric 

Minutes 

SSSSSS3S3S 



N = 4 

0 

0.000000 

0.000000 

0.000000 

5 

13.746736 

13.746736 

13.746736 

10 

20.161917 

20.161917 

20.161917 

15 

12.828962 

12.828963 

12.828961 

20 

-12.952953 

-12.952950 

-12.952956 

25 

-59.582233 

-59.582227 

-59.582237 

30 

-126.855544 

-126.855533 

-126.855551 

35 

-211.993191 

-211.993176 

-211.993200 

40 

-309.986022 

-309.986003 

-309.986034 

45 

-414.220565 

-414.220544 

-414.220579 

50 

-517.304388 

-517.304365 

-517.304403 

55 

-611.988666 

-611.988644 

-611.988681 

60 

-692.072834 

-692.072815 

-692.072847 


Table 2-9. Comparison Test Between N = 4 and N = 10 Steps 







is functioning properly, and that it interfaces properly with the State 
Vector Transformation module SVX to produce correct sequences of commands 
to the flat floor. By choosing a coarser step in the numerical integration 
process, OMV is able to complete all the necessary computation within a 
major cycle, without compromising on the accuracy. 



Chapter 3 

STATE VECTOR TRANSFORMATION MODULE (SVX) 

3.1 INTRODUCTION 

The State Vector Transformation Module (SVX) is an interface between 
the OMV simulation model and the mobile base (TOM-B) of the flat floor 
simulation system. We can imagine the OMV simulation to be a free flying 
vehicle in space under human operator control, and at any particular 
instant, its state can be summarized as a fourteen-component vector called 
the state vector S. SVX takes this state vector as an input and generates 
an appropriate string of commands that is transmitted to TOM-B with the 
stipulation that if TOM-B executes this command string exactly, then the 
mock-up module mounted on TOM-B will exactly replicate the motion of the 
OMV as perceived by the operator. 

References [14,17] are reports that pertain to the various aspects of 
the OMV. From these reports, the various components that make up the state 
vector can be deduced and are presented below: 


Component 

Symbol 

Meaning 

1 

X 

Position of the target vehicle relative 

2 

Y 

to the OMV in local vertical frame LVF 

3 

Z 


4 

Vx 

Relative velocity of the chase vehicle 

5 

v y 

in LVF 

6 

v z 


7 

Lx 

Angular momentum vector in LVF 

8 

L y 


9 

L Z 


10 

91 

Attitude quaternions in body frame 


11 


12 

12 Q3 

13 P4 

14 m Mass of OMV 

It is often more convenient to consider the state vector to be made up of 
the following four vectors: X = [X,Y,Z] T , V = [V x ,Vy,V z ] T , L = [L x ,Ly,L z ] 

and the unit quaternion q = [qi,q2»q3»q4] T - 

As mentioned earlier, the required command string must be derived 
from this state vector, and is transmitted to TOM-B as seven 16-bit words. 
The last word can either be a zero or a one, which is interpreted by the 
TOM-B Executive as rate or position control respectively. A brief explana 
tion of the command string is shown below: 


Component 

Position Control 
Symbol Meaning 

Rate 

Symbol 

Control 

Meaning 

Coord. System 

1 

y 

yaw of TOM-B 

• 

y 

yaw rate 

body frame 

2 

X 

position of 

v x 

velocity of 

LVF 

3 

Y 

TOM-B 

Vy 

TOM-B 


4 

Z 

pos of pivot 

V z 

vel of pivot 


5 

P 

pitch angle 

* 

p 

pitch rate 

body frame 

6 

r 

roll angle 

* 

r 

roll rate 


7 

1 

pos. control 

0 

rate control 


Before the 

detai led 

analysis is presented, it 

is necessary to 

define the 

various coordinate 

systems used. 





3.2 COORDINATE SYSTEMS 

Several coordinate systems are used in this software module. 
Specifically, motion of the OMV is described in Local Vertical Frame (LVF) 
while the orientation of the OMV is described in body frame. Similarly, 



the position and velocity of the mobile base TOM-B is described in floor 
coordinates while the orientation of the mock-up module and TOM-B are 
described by the respective body frames. 

A. The Local Vertical Frame (LVF) 

Imagine a circular orbit that is inclined at an angle i with respect 
to the equatorial plane. A Local Vertical Frame is a non-stationary frame 
that has its origin at a point on this orbit such that: 

(i) Its Z|_ axis is directed away from the earth's center, 

(ii) Its X|_ axis is directed tangential to the orbit and is perpen- 
dicular to its Zl axis, and 

(iii) The Y|_ axis is directed parallel to the angular momentum vector, 
as shown in Figure 3.1. 

A subscript L will be used to indicated quantities defined in this coor- 
dinate system. 

B. The Floor Coordinate (F) 

The floor coordinates has its origin at one corner of the flat floor 
as shown in Figure 3.2. Its Xp axis is directed along the width of the 
floor, while the Yp axis is directed along the length of the floor. 
Naturally, Zp axis is directed vertically up. 

C. The TOM-B Frame (B) 

This coordinate system is fixed with respect to the mobile base, and 
has its origin at the center of mass of the mobile base. Its Xg axis is 
directed towards the front of TOM-B, while its Zg axis is parallel to the 
Zp axis of the flat floor. A third axis Yg is chosen so as to form an 
orthogonal right-handed coordinate system, a top view of which is shown in 
Figure 3.3. 
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D. The Mockup Module Body Frame (M) 

It assume that the mockup module resembles the OMV in shape (that is, 
not unlike a pancake). The origin of its body frame coincides with its 
center of mass, and the axis is directed towards the front of the 

module. Initially, at the start of the simulation, the Zm axis is chosen 

to be parallel to Zp, and the appropriate orthogonal axis is chosen as its 
Ym axis, as indicated in Figure 3.4. 

3.3 ANALYSIS 

It is obvious that the position and attitude from the state vector 
are relative quantities. Thus, initial conditions at the start of the 
simulation must be known. Figures 3.5 and 3.6 shows the initial state of 

the mobile base and mockup module at the start of the simulation. The 

quantities a, c, 1, h and o can be obtained from measurement. 

A necessary initial condition is that the operator must leave the 
hand controllers in the neutral position for at least one second so that 
the initial position of the OMV [X 0 ,Y 0 ,Z 0 ] T can be obtained. It is also 
assumed that the initial orientations of both the OMV and mock-up module 
are set in their home position. If the notation r, p, and y is used to 
indicate the roll, pitch, and yaw of both the OMV and the mock-up, then, 

[ r 0MV, POMV, yOMV ] T = [ r M, Pm, m ]T = [ 0, 0, 0 ] T 

It is obvious that the corresponding axes of the coordinate frames M, B and 
F are all parallel at this point in time. At any later time, the position 
of the OMV can be calculated from the state vector: 
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Figure 3-5. Initial Position (top view) 
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Here, Si, S 2 , and S 3 are the first three components of the state vector. 
This position is measured relative to the starting point in the beginning 


of the simulation, and can be transformed to the position of the mockup 
module in floor coordinates using the equation: 
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Equation [I] governs the transformation of the position vector of the OMV 
in LVF to a position vector for the mockup module in floor coordinates, 
based on the initial conditions and the first three components of the state 
vector. Given that the instantaneous orientation of the module is [r^, pm, 
rM]"*" as shown in Figure 3-7 and 3-8 the position of TOM -8 [Xp, Yp, Zp]^ in 
floor coordinates is given by: 
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’x M - (c + lcos( p) )cos(y ) 

Y F 

= 

Y m - {c + lcos(p) }sin(y) 

_ Z F 
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Note that Zp is the height of the center of mass of TOM-B from the floor (a 
constant quantity), and is not of interest here. Instead, the quantity of 
interest is Z, which is the height of the pivot point from the floor as 
shown in Figure 3.6, and 


Z = Zm - lsin(p) 


It follows that the velocity of TOM-B and the pivot point is given by 




Figure 3-8. Pitch and Roll of Mock-Up Module 
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The above transformations take care of the position and velocity quan- 
ti ties . 

The quaternions qi, q2, d3, Q4 from the state vector specifies the 
OMV's attitude in body frame, as discussed in References [18,19]. At any 
instant, its orientation is given by [10]: 

[ r. p, y ] T = a[O x ,O y ,O z ] T 


where 


a = 2 cos-1 (q 4 ) 

[ O x , O y , O z ] T = (iq x + jq 2 + kq 3 ) / (q x + q 2 + 

while their rates are wj$ = [wi, W2» w 3 ] T which can be calculated in the 
following manner: 

Since the angular momentum vector L = [L x , Ly, L z ]^ from the state 
vector is expressed in LVF, it is necessary to transform it to body frame 
using the equation: 


LB » A L 


here A is the direction cosine matrix which can be constructed from the 
attitude quaternions qi, q2, 93, and q 4 


d4 + qi - Q2 ” Q3 2(qiq2 + q3d4) 2(qiq 3 - q 2 q 4 ) 


A s 


2(qiq 2 - q3q4) 
2(qiq3 + q 2 q 4 ) 


q4 - qi + q 2 - q3 2(q 2 q 3 + qgq 4 ) 

2 ( q 2 q3 - qiq 4 ) d4 ~ dl - d2 + d3 


Knowing the moment of inertia tensor I, one can calculate the angular rates 


wb = [wi, W 2 , W 3 ] t 

= I' 1 L B = I’ 1 (A L) 


Thus, one has all the needed information from the state vector to yield the 
necessary position or rate control commands. 


3.4 ALGORITHM 

The algorithm for SVX makes use of all the transformations described 
in the above section. Essentially, the algorithm uses the state vector and 
depending on the value of MODE, generates the appropriate command string 
CMDRAW. 

Case 1 MOOE <> 0 (position control) 

In this case, both orientation and position of the OMV are updated. 

A transformati on is made to yield the position of the center of mass of 
TOM-B using equation [I] through [III]. The orientation of the mock-up 
module is obtained using equation [VI]. Using the previous notation, a 
seven element vector 


Cy, X B , Y b , Z, p, r, 1]T 

is generated. Each element of this vector is suitably and round off to the 
nearest integer (16-bit word) and is the sole output of the SVX module. 

Rate information is not of interest when the system is in position control. 


and is therefore not transmitted. Throughout this module, the scale fac- 
tors for all angular and displacement quantities are 10^ and 10^ respec- 
ti vely. 

Case 2 MODE <> 0 (rate control) 

In this rate control mode, it is still necessary to update the orien 
tation (equation [VI] although it is no longer necessary to update the 
position of the OMV. The velocity of TOM-B in floor coordinates is deter- 
mined from equation [IV] while the rates for roll, pitch and yaw are deter 
mined using equations [VII] through [X]. The seven 16-bit word command 
string is: 

Cy» *b, Y B> z > P» r > °^ T 

As before, each component of this vector is similarly scaled and rounded 
before returning. 

Case 3 MODE <> 0 and MODE <> 1 

In this case, MODE is set to 1, and position control is assumed. 

3.5 IMPLEMENTATION 

This algorithm is implemented as a subroutine named SVX (S, CMDRAW, 
MODE) where the three items on the parameter list are the state vector out 
put command string and control mode respectively. 

The subroutine is implemented in FORTRAN 77, and the usual 
programming practices are adhered to. Most of the major steps are either 
properly documented in the form of COMMENT statements or implemented as 
subprograms, following a modular design approach. Whenever possible, 
structured codes are used unless severe degradation of execution speed may 


result. 


SVX is compiled and tested using a IBM Personal Computer, and the 
source code, on completion of the testing, is uploaded to the POP 11/34 
computer at MSFC. Appendix 6 shows a complete listing of this module. A 
more detailed description of the testing procedure will be presented later 
in this section. 

A local counter (COUNT) is initialized at load time, and updated 
during execution to enable SVX to determine the initial state on start up. 

During this period, other tasks are carried out as an integral part of the 

initialization process. This includes reading a file (SVXINT.DAT) for the 
values of c, 1, a, h and o, as well as the inverse of the moment of inertia 
tensor I“l. 

This module assumes that the operator will, at start up, leave the 
hand controller at a neutral position for at least a second. During this 
interval, the initial state of the OMV is recorded, and the vector E where 

E = [Ei, E 2 , E 3 ]T 
- [c + 1 - X 0f a - Y 0f h - Z 0 ] T 

is calculated. The roll, pitch and yaw of both the OMV and the mock-up 

module are initialized to zero during this process by invoking subroutine 
ZERO. 

Subsequent calls to SVX causes a seven 16-bit command string in an 
INTEGER array called CMDRAW to be produced. Computation here depends on 
the value of MODE. 

When MODE is non-zero, position control is assumed. SVX invokes 
subroutines QTRPY and UPDPOS to calculate the desired orientation and posi- 
tion of the OMV. A transformation is then made to determine the required 
position (of the mobile base TOM-B in floor coordinates) and orientation 



(of the mockup module in body frame). Since the value of MODE cannot be 
changed in the course of a simulation, no rate information is calculated or 
retained. 

When MODE is zero, rate control is used. First, QTRPY is called to 
calculate the orientation of the OMV; its position is not computed because 
it is not of interest while in the rate control mode. The direction cosine 
matrix A is formed by invoking subroutine DIRCOS, and a simple matrix 
multiplication transforms the angular momentum to body frame. Finally, the 
velocity of the OMV (from the state vector) is suitably transformed to 
yield the velocity of TOM-B in floor coordinates, and the appropriate com- 
mand string assembled. 

When MODE is neither zero nor one, it is set to one and defaults to 
position control. One frequently used subroutine in both modes is DECOMP 
which takes the state vector S and decomposes it to form the vectors X, V, 

L and q which correspond to the displacement, velocity, angular momentum 
and the unit quaternion vectors respectively. Throughout this module, no 
attempt is ever made to ensure that the magnitude of q is unity. 

To ensure that SVX generates the correct command string, a series of 
tests were conducted using the IBM PC. First, a simple State Vector Editor 
is written. This editor allows one to create and edit, interactively, 
state vectors which are placed in sequence in a disk file. Next, a simple 
main program is written and linked to the SVX module. The main program 
consists of a driver loop that reads each state vector from the disk file 
and invokes SVX. The command string outputted by SVX is sent to a printer 
and the process is repeated until the file of state vectors is exhausted. 
This simple arrangement allows one to verify the correctness of SVX without 
disturbing it. 



Since it is difficult, if not impossible, to represent the results 
graphically in three dimensions, state vectors are chosen such that one can 
easily displays the results in two dimensions. By way of example, a 
sequence of 60 state vectors of the form: 

[0,0,0, 0,0,0, 0,0,0, 0,0,sin(7.5) ,cos(7.5) , 1500]T 

is generated. This set of state vectors simulates 50 seconds of run time 
in which position control is used. The meaning of this state vector is 
that the OMV is to remain stationary, but executes a yaw at a rate of 15° 
per major cycle (0.1 second). Here, we have assumed that the OMV is a disk 
shaped object having a uniform mass distribution and a constant mass of 
1500 pounds. Note that in case of position control, the angular momentum 
vector is inconsequential, so a null vector is used. These figures may not 
be very realistic, but they are adequate for testing the SVX module. 

Figure 3-9 shows the result of a portion of the output command string. In 
this and subsequent figures, a circle or dot indicates that the position of 
the center of mass of TOM-B in floor coordinates, while an attached arrow 
shows its yaw. This figure depicts that TOM-B moves in a circular path and 
its yaw is changing at a rate of 15° per major cycle. It is noted that the 
radius of the circular path is equal to the distance between the centers of 
mass of TOM-B and the mock-up module. Thus, the mock-up module would be 
spinning about its Zm axis at the same rate, exactly as expected. 

When the state vectors are changed to 

[0.5, 0,0, 0,0,0, 0,0,0 0,0,sin(7.5) ,cos(7.5) , 1500] T 

in position control, the path of TOM-B is shown in Figure 3-10. In this 
figure, TOM-B attempts to move in a circular path with a net displacement 




Figure 3-9. Position of TOM-B in Floor Coordinates 



of 0.5 feet per major cycle. It is easy to conclude that the mock-up 
module would be rotating about its Zm axis and translate along the axis 
simultaneously, as demanded by this state vector. 

3.6 RESULTS 

Other similar tests have been conducted. For example, the state vec 
tor in the beginning of this section has been input for rate control, and 
the result is plotted in Figure 3-11. This and simular results have 
demonstrated that the module SVX is functioning properly and that correct 
command strings are obtained. One must remember that the outputs of this 
module are commands to TOM-B, indicating the desired position, (or veloci- 
ty) and attitude (or angular rates). The proper interpretation, and sub- 
sequent execution, of these commands are performed by the TOM-B Executive, 
and is outside the scope of the SVX module. 



Figure 3-11. Velocity Components of TOM-B 


Chapter 4 


MOBILITY BASE ON-BOARD CONTROL LOGIC 
TOM-B 

4.1 INTRODUCTION 

TOM-B is the control software that drives the mobility base. A 
description of the mobility base has been given in Chapter 1, and will not 
be repeated here. 

TOM-B is designed to perform position or rate control over the mobi- 
lity base. During development and testing, position control was used. The 
command structure coming from six could consist of a sequence of 6 numbers, 
each of which specifies the desired position and orientation of the vehicle 
when the command is executed but because of communication bandwidth, the 
command string consists of a positional increment, which must be added to 
the current position to yield the desired position. Further, the most 
efficient mode of transmission is in integer format and this format is 
adopted here. It is understood that for positional quantities (such as X, 
Y, and Z) the unit used is 0.001 inch, while for the remaining quantities 
(angular), a unit of 0.1 degree is used. By way of example, the command 
string: 

10 0 20 0 0 0 

is interpreted such that TOM-B move along X axis 0.01 inches from the 
current position, and rotate by 2 degrees about its Z axis. All other axes 
remain unchanged. Symbolic names are used to represent each of these quan- 
tities in the command string, the command transmitted to TOM-B is of the 
form: 

CMD-X, CMD-Y, CMD -THETA, CMD-Z, CMD-P, CMD-R 

Essentially, based on the desire position/orientation and the current 


position/orientation, one can calculate the required impulses f x and fy. 
This is the required impulse that moves TOM-B from the present position to 
the desired position, and is expressed most conveniently in floor coor- 
dinates. This impulse is translated into the corresponding impulses FX and 
FY, which are impulses that must be exerted by TOM-B. This is necessary 
because at any particular moment, the body-centered coordinate system 
defined with respect to TOM-B may not be lined up with the floor coor- 
dinates. Once FX and FY are known, the individual impulses FX1, FX2, FY1, 
and FY2 to be exerted by the appropriate thrusters are determined. From 
these impulses, one can calculate the firing times of these thrusters, 
since they cannot be throttled. The firing times are then suitably scaled, 
and the appropriate numbers loaded into the corresponding down counter. A 
control signal is then sent to fire the thrusters, as shown in Figure 4-1. 

Figure 4-2 shows the hypothetical position and orientation of TOM-B 
when the position and orientation of TOM-B is given by the vector (x,y, ) 
determined from the navigation system. Here is the orientation of the 
vehicle. The desired position and orientation is dictated by the command 
string (Xcmd> y CMD* CMD) such that the vehicle will be at this position at 
the end of the current major cycle. The required impulse to accomplish 
this is given by: 


f x = mag(X,XcMD>Vox) 

f y = mag(Y,YcMD» v OY) 

where f x , fy are the required impulses along X and Y directions in floor 
coordinates. V is the velocity of the vehicle, also expressed in floor 
coordinates. It is noted that V ox , and V 0 y are obtained from the accelero 
meter readings V' x and V'y using the transformation: 
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Figure 4-2. TOMB Orientation Angle 0. 
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and the function g, is given by: 

2 

g(X,XcMD» v ox) s T - X if X is non-negative 

g(X,X C MD»V 0 x> 3 - v ox + (Vox * 2a(X CMD - X))l/2 otherwise 

a 

where 

X 2 • T2 - g(X CMD - X • V 0X T) . 

a 

Here, a is the magnitude of the acceleration produced when one pair of 
thrusters is fired simultaneously in the same direction, and is approxi- 
mately equal to 0.1 ft/sec^. T = 0.1 is the major period. Note that the 
impulses f x and fy are defined relative to the floor coordinates. To 
determine the actual impulses F x , Fy that TOM-B must exert to produce the 
same displacement, we use the transformation: 



cos 0 
sin 0 



where 0 is the orientation of the vehicle as determined by the navigation 
system. 



4.2 CONTROL LAW 


Once the impulses F x and Fy are known, then the individual impulses 
F x i, F x 2 , Fyi, and Fy 2 that each thruster must produce can be calculated 
[13]. The notation as shown in Figure 4-3. Wherever a negative quantity 
is encountered, the directly opposite thruster will be used. Obviously, 
one must have the relation: 


F x 3 F xl + f x2 
F y 3 F yl + F y2 

Note that not only must the impulses produce the required translational 
displacement, but also must produce the necessary angular displacement. We 
define the required torque T 0 by the relation: 

T 0 * 2J ZZ ( 0 CMD " 9 ) / T2 

where T is the major period and J zz is the principle moment of inertia 
about Z-axis of TOM-B [5]. It is prudent to consider the following two 
cases. 

Case 1. F x ^ Fy 
In this case: 


F yl 3 F y / 2 + T 0 / (2L y ) 

F y2 3 F y " F yl 

If one defines a quantity F x to be 

F x 3 (T 0 + ( F y2 - F yl)Ly) / C2L X ) 


then 




F xl = F x / 2 + F x 
f x2 = F x - F xl 


Cass 2 . F x y Fy 
In this case, 


F xl = F x / 2 + To / <2L X > 
F x2 = F x - F xl 

If one defines another quantity T' 0 such that: 

T'o ■ T 0 + (F x2 - F x1 )L x 


then, 


F yl « F y / 2 + T'o / (2Ly) 

F y2 = F y - Fyl 

These impulses must be converted into the corresponding firing times T x i, 
T x2 , Tyi, and Ty 2 , respectively because the thrusters are not throttleable. 
These can be accomplished using the formula: 

Txj = F xj / ma 

T yj * F yj / ma 

for j = 1, 2. Here, ma (mass times acceleration) is the thrust developed 
by each thruster. Recall that a negative T x j means that opposite thrusters 
will be used. 

4.3 TOM-B PROCESSING LOGIC— ALGORITHM 

In this section, the high level control logic is discussed fully. 

The name of the software is TOMC. This is to differentiate between the 


hardware TOM-B. The code is written in FORTRAN and MACRO-II (Appendix 5). 

A top down design is used throughout. 

The main program of the control logic is shown in Figure 4-4. The 
initialization procedure consists of the following steps: 

a) A routine is used to set up a schedule to interrupt the system 
ten times every second. The interrupt service routine must: 

1) Interrupt the incoming command string, 

2) Determine the present position and orientation of TOM-B using 
the navigation system, 

3) Get the buffers containing the accelerometer and gyro 
readings. Note that the position for the other three axes 
(Z, pitch and roll) will also be determined by this service 
routine. 

Thus, updated information is always available in any given major 

cycle. 

b) Static quantities (such as physical dimensions of the vehicle 
which are not expected to change) are initialized. 

c) A data file is opened and accessed so that dynamic quantities 
such as mass of fuel, number of thruster pairs per side, thrust 
that will be developed by each thruster, calibration data, scale 
factors, etc., are initialized. This is an efficient design, as 
the system may be subject to further modifications, or the 
experimental condition may change (e.g., a different module may 
be mounted, causing a change in the mass of the vehicle). Under 
this circumstance, the data file is modified offline, without 
having to change and recompile the entire software. 


After the initialization phase, the balance of the main program 
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involves intercepting the command string once every 0.1 second, and , exe- 
cuting this command string until a command to stop is encountered. When 
this happens, preliminary shutdown procedures (such as turning off all 
thrusters) is carried off before the final system shutdown. 

The processing of a major cycle is carried out in a procedure called 
MAJOR, as shown in Figure 4-5. On entering this procedure, appropriate 
memory locations are accessed and the current position and orientation of 
TOM-B are determined. The command string is examined first to see if any 
thrusters must be activated. A separate routine called THRUSTER performs 
the necessary thruster logic. When this subtask is completed, the balance 
of the command string is examined to see if it is necessary to move any of 
the stepping motors which control the remaining axes (Z, pitch and roll). 
The procedure MOTOR performs the necessary stepping motor control logic. A 
waiting procedure is implemented to place the processor in a dormant state 
until the next command string is intercepted. A higher priority is 
assigned to thruster logic. This is deliberately done because of the 
nature of the thruster hardware logic. An appropriate number is placed in 
the corresponding down counter and a control signal is issued to fire a 
thruster. The hardware fires the thruster and decrements the counter until 
its contents are zero, after which the thruster shuts down. During this 
interval the processor performs other tasks, and need not wait until the 
firing cycle is completed. For this reason alone, thruster logic is pro- 
cessed first is procedure MAJOR. 

4.4 TESTING AND VERIFICATION 

Verification of TOM-B was accomplished by a series of measurements 
and tests conducting using the mobility base. These series of measurements 


MAJOR 




Figure 4-5. Control Software - Major Cycle 





were lengthy and involved interaction with the hardware. Although handling 
and adjusting hardware components were outside the scope of the contract, 
UAH has provided personnel to perform these minor operations, under the 
supervision of MSFC personnel, to expedite the testing procedure. Because 
of the frequent hardware modification/upgrade and because of concurrent 
time needed by ESSEX for their measurements, it was not possible for UAH to 
have a reasonable block of uninterupted machine time for testing purposes. 
Frequently, it is necessary to schedule our tests between ESSEX'S runs. 

More frequently, our tests have to be suspended because of hardware una- 
vailability or failures. 

A series of tests were conducted initially to ensure that the TOM-B 
initialization procedure was corrected. This was done by modifying the 
code to display all critical parameters such as scale factors/orbits of 
the gyro end accelerometers, firing table, etc. The interrupt routines 
were also thoroughly tested on-line. The result was that several parame- 
ters have to be tuned, but this was easily accomplished since all critical 
parameters were placed in data files, and as such they are easy to modify 
without disturbing the code. 

Several of the components on the mobility base must be calibrated in 
order to obtain some of the parameters. These include the gyro and the 
accelerometers. For proper operation, the precise scale factor and offsets 
of these components must be obtained in order to correlate the outputs of 
these sensors to actual vehicle parameter (position, speed and orientation 
in the appropriate units). Figure 4-6 shows the gyro/accelerometer package 
[20]. An optimal place to mount these packages would be at the e.g. of the 
mobility base. This was accomplished mostly by trial and error method, and 
special software was developed for this purpose. 



Figure 4-6. Accelerometer & Gyroscope Mounting 



The gyroscope and the accelerometer have been bench tested, but our 
runs showed that an on-vehicle calibration is necessary. The gyroscope was 
calibrated in the conventional manner. A seperate calibration progress 
called ACE was developed to permit date acquisition and analysis. The pro- 
cedure developed is as follows: 

a) Allow the system to warm up to operating temperature. 

b) All air handles to the facilities have been disabled so that 
drafts would not cause any extraneous motion. This turned out to 
be an important consideration, especially when a full scale OMV 
mockup was mounted on the mobility base. 

c) ACE was commanded to fire an appropriate set of thrusts, causing 
the mobility base to execute a pure rotational motion about its 
Z-axis. The firing time was recorded. 

d) The angular displacement in radians during the thruster firing 
was recorded. 

e) When the thrusters ceased firing, the angular displacement and 
time required until the mobility base ceased rotation were also 
needed. 

From these data, it is possible to deduce the kinetic coefficient of 
rotational friction (which turned out to be quite small) and the proper 
scale factor (and offset) of the gyroscope. Thus, one can correlate the 
gyro output to angular displacement. Figure 4-7 shows one such set of 
calibration data. 

Similar procedures were used to calibrate the two accelerometers 
mounted along the X and Y axis of the mobility base respectively. In this 
instance, however, the appropriate thrusters were selected to produce pure 
translation along a single axis instead. Several interesting phenomenon 




were observed: 


a) The e.g. does not line along the symmetrical axis of the vehicle. 
It was necessary to counter-balance the mobility base with lead 
bricks in order to obtain translational motion without a rota- 
tional component. 

b) The kinetic coefficient of friction was quite large. The test 
procedure was to enable the thrusters for two seconds, measure 
the displacement di, after which time the mobility base was 
permitted to coast to a stop. The displacement d2 and time t 2 
were recorded. In 60% of the the trials, d2 was no greater than 

d l* 

c) The floor is not flat. With the air handles off, there was no 
significant air current in the facility. When the mobility base 
was put in certain areas of the floor, it had a tendency to drift 
in a consistent direction, but the drift rate although observable 
is very small. 

Figure 4-8 shows the a typical calibration curve of one of the acce- 
lerometers. Both accelerometers behave quite identically so that this 
figure is quite typical. Immediately several problems are evident. 

a) The signal to noise ratio is unacceptable, as can be estimated 
from this diagram. Remember time t * 0 was the time when the 
thrusters commenced firing. 

b) A slope change was always observed approximately ± seconds after 
time t = 0. This change of slope represents the fact that the 
thrust level drops after i seconds of firing. This is further 
substantiated by a change in the pitch and is detectable by 
hearing. This drop in level is an indication that the phlenum is 





not able to supply air at the designed rates to the thrusters. 
This could be a result of an engineering design change in which 
additional thrusters have been added to the phlenum. 

c) The data shows a lot of scattering. This is due to the excessive 
vibrations transmitted to the accelerometer when the thrusters 
are enabled. 

The combination of poor signal to noise ratio, a drop in thrust level 
after 0.5 seconds and noise means that at best, one may extract marginal 
rate data from the accelerometer outputs, and would entail the use of 
various smoothing, fitting and integration techniques. Thus, attempting to 
obtain position data by further integration would be counter productive. 
These observed problems, as well as a recommendation for a independent 
position feedback subsystem, was reported to MSFC. 

It is at this point in time that the contractual period was up, and 
the facilities was scheduled to shut down for major hardware modification. 

4.5 RESULTS 

Although we were not able to complete testing the software, several 
important tasks have been accomplished. First, attitude control using gyro 
output was completed. During some tests, we were able to point the mobi- 
lity base in any desired direction and maintaining this direction. This 
indicated that the software is exercising positive control for this axis. 
Since the accelerometer data are processed in the same fashion, all that 
would be needed to close the loop was to implant a position feed back sub- 
system. This was completed in January of this year. Mr. Ralph Kissel of 
MSFC wrote the necessary software to control this sensor as well as the 
analysis logic to process the data. These models were integrated to TOM-B 


and the system tested. Two methodologies were used to obtain the rate 
data. The first was to use the accelerometer output, while the second 
method was to compute the rate by computing the time derivatives of the 
position data. 

After integrating the position feed back system, TOM-B works as 
expected. In a test run, the mobility base was instructed to translate 
along its x-axis by 5 cm, execute yaw of 30° and then hold that position 
and orientation. The mobility base did just that, indicating that the 
software does indeed exercise closed-loop control over the mobility base. 
One disturbing observation is that to execute this maneuver, most of the 
thrusters are firing, an indication that further optimization of the 
control logic may be needed. The complete listing of TOM-B is given in 
Appendix 7. 
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OMV Translational Equations of Motion 



Appendix 1. 


OMV Translational Equations Of Motion 

Consider a target vehicle orbiting the earth with an angular velocity w 
and an orbit radius of R Q . We can define a local vertical frame (LVF) at the 
center of gravity of this vehicle as shown in the figure below : 



Here, Xg, Yg and Zg are the three orthogonal axes of the LVF. We can imagine 
that the center of the earth may be considered as the origin of the inertial 
coordinate frame. We can chose the axes of this coordinate system as shown. In 
particular, Yg is parallel to Yg. We shall use the subscript L to denote those 
quantities that are expressed in the LVF, while the subscript E shall be used 
for those quantities expressed in the inertial frame. The point C in the 
above figure represents the center of mass of the chase vehicle (OMV) 



The equation of motion of the chase vehicle is easily deduced from New- 
ton’s second law, namely, 

M C R - F g + F c (1) 

This equation is written in the inertial frame. Here, M c is the mass of the 
chase vehicle, is the gravitational force exerted on the vehicle by the 
earth, and F c is the control force exerted on the vehicle from the on-board 
thrusters and jets. The objective of this exercise is to derive the equation 
of motion in terms of r and its time derivatives. Namely, we wish to express 
the motion of the chase vehicle (OMV) in local vertical frame. This choice 
turns out to be very convenient for docking maneuvers. 

From the above figure, it is obvious that 

R * R 0 + r E (2) 

it follows that 

R * R 0 + r E (3) 

Since the LVF is a rotating frame, we can use the operator : 

{ d/dt }g « { d/dt + w x 

Applying this operator to r twice, we have 

r E * r L + w x r L 

and 

r E = d/dt (f E + w x r E ) +wx (r^+wx r^) 

* *r E + v x r E + w x r E + w x (v x r E ) 

= r E + 2w x f L + w x (w x r E ) 


( 4 ) 


From equations (3) and (4), we have : 


R o + r E 

R 0 + + 2w x r^ + w x (v x rj ) 


Furthermore, for a circular orbit, 


R 0 + w R 0 = 0 


therefore, 


* - W ^R 0 + + 2w x + w x (w x r^) 


(5) 


It is clear at this point that the equations of motion (1) can be rewritten in 
terms of and R Q and their time derivatives. Thus the subscript will be - 
dropped from here on. Recall that 


so that 


Thus, 


R = 


R 2 = 



(R 0 + r) . (R 0 + r) 

Ro + r 2 + 2R 0 .r 

R o + 2R o* r 

Ro (1 + 2(R 0 .r) / R 2 ) 

R“ 3 ( 1 + 2(R Q .r) / R 2 )~^/ 2 

R; 3 {1 - 3(R 0 .r) / R 2 } 


F g = -(GM e M c /R 3 ) R 

= -(GM e M c /R 3 ) (R 0 + r) (1 - 3(R 0 .r) / R 2 ) 
* -w 2 M c (R 0 + r) (1 - 3(R c .r) / R 2 ) 

-w 2 M c (R 0 + r - 3(R 0 .r/R 2 )R Q 


( 6 ) 


since for a circular orbit, 


2 

w * 


GM e /R 3 . Substituting equations (5) and (6) 



into ( 1 ) t we have : 


M c (-w 2 R g + r + 2w x f + v x (w x r) ) = F - M c w 2 (R Q + r - 3(R 0 .r)/R* } 
If we define A = F c / M c , then we have : 

-w 2 R q +r +2vxr+wx(wxr) 

which, after re-arranging, gives : 

•• 2 
2w x r - " 


= A - w 2 R q - w 2 r + 3w 2 (R 0 .r/RQ>R 0 


r = A - 


w^r - w x (w x r) + 3w 2 (R Q .r/R*)R, 


o 7 o 


(7) 


Now, we shall state r, R Q and v in cartesian coordinates. It is explicitly 
assumed that the unit vectors i, j and k are directed along X^, Y^ and axes 
respectively. Thus, 


r 

R c 

w 

A 


[X, Y, Z] T 

[0, 0, r 0 ] t 

[0, w, 0] T 

[ Ajj , Ay , A z ] 


and 


and it can easily be shown that 


2vxr 

* [ 2wZ, 

0, -2wX j T 


w x (w x r) 

* [ -w 2 X , 

0, -w 2 Z] T 


3w(R 0 .r/R2)R D 

= [ o, 

0, 3w 2 Z]^ 

and 

w 2 r 

■ [ w 2 X, 

w 2 Y, w 2 Z] T 


substituting into 

' equation (7) yields 


[X, Y, Z] T 

= [ -2wZ, 

0, 2wX] T + [ v 2 X, 

0, w 2 Z] T 


+ [ -w 2 X, 

-w 2 Y, -w 2 Z] T + [ 0, 

0, 3w 2 Z] T 


+ [ A x , 

Ay. A z ] T 




or 


X = A x ~ 2wZ 

V' = A y - w 2 Y (8) 

Z = A z + 2wX + 3 w 2 Z 

Equation (8) is the equation of motion of the chase vehicle relative to the 
target vehicle in local vertical frame. 
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..inc# 1 7 Microsoft FORTRAN’77 V3.13 8/05/83 

1 SPAGESIZE: 56 

2 STITLE: '«< 0 M V PLOT >»’ 

3 C 

4 C 

5 C 
() C 


7 C 

8 C 

9 C 

10 C 

11 C 

12 C 

13 C 

14 C 

15 C 

16 C 

17 C 

18 C 

19 C 

20 C 

21 C 

22 C- 

23 C 

24 C 

25 C 

26 C 

27 C 

28 C 

29 C 

30 C 

31 C 

32 C 

33 C 

34 C 

35 C 

36 C 

37 C 

38 C- 

39 C 

40 C 

41 C 

42 C 

43 

44 C 

45 C. 

46 C- 

47 C 

48 C 

49 C 


Program : 0 M V P L 0 T 


by 


Dr. W. Teoh 


UAH 1984 


This is a graphical package that accepts a command string 
and uses this information to generate and display the 
position and orientation of T0M_B and the attached mock- 
up module in two dimensions. One can choose to display 
either the top or side view of the system. 

This package is developed in FORTRAN 77 to run on an IBM 
PC with at least 128K of RAM, and fitted with a TECMAR 
CRAPHICS MASTER board. An IBM Monochrome monitor is used 
for the actual display. The resolution in this work is 
chosen to be 640 x 350. 


SUBROUTINE SIDEVEW (H, X, P) 


This procedure produces a side view of T0M_B and the 
attached mock-up module. The perspective is always in 
the direction of +1 axis of the body fixed coordinate 


«< 0 M v 


PLOT >» 


D Line// 1 

50 C 

51 C 

52 C 

53 C— 

54 C 

55 C 

56 

57 

58 

59 

60 

61 C 

62 

63 

64 

65 C 

66 C 

67 

68 

69 C 

70 C 

71 C 

72 

1 73 

1 74 100 

75 C 

76 

77 

78 C 

79 

80 

81 C 

32 

83 

34 C 

85 

86 

87 C 

88 C 

89 C 

90 

91 

92 

93 

94 

95 

96 

97 C 

98 C 
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system 


REAL * 3 H X P C S 

REAL XF0RM( 3^3)1 SDFORM(3,3), V0(3,10), V(3 t 10) 

REAL ROT (3,3), FLOOP.(3,3), Vl(3,10) 

REAL CC, DD, LL, RR, WW, TT 

INTEGER FLAG, N, CLR, EF.EEF, PRTFG 

COMMON /MG/ FUG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM, SDFORM, VO, VI 
COMMON /ME/ EF, EEF, PRTFG 


N = 10 
AA = 1.0 


*** define mock-up module shape at origin 


DO 100 

K = 

1, 

V(3, 

K) 

= 

CONTINUE 


V(1,D 

s 

TT 

V(2 , 1 ) 

s • 

-DD 

V(1 ,2) 

S ' 

-TT 

V(2 , 2) 

= ■ 

-DD 

V( 1 ,3) 

— 

-TT 

V ( 2 , 3 ) 

= 

DD 

V( 1 ,4) 

s 

TT 

V(2 ,4) 

= 

DD 


« point A » 
« point B » 
« point C » 
« point D » 


*** rotate it by P radians 

CALL SINCOS (P, S, C) 

CALL NOTHNG (ROT, 3) 

ROT (1,1) * C 

ROT( 1,2) = -S 

ROT (2,1) = S 

ROT (2,2) * C 

CALL XMUL (ROT, V, 4) 


*** calculate translation 



0 M V PLOT »> 


,ine# 1 
99 C 
100 
101 

102 C 

103 C 

104 C 
103 
106 

107 

108 

109 C 

110 C 

111 c 

112 C 

113 C 

114 

115 

116 C 

117 

118 

119 C 

120 
121 

122 C 

123 

124 

125 C 

126 

127 

128 C 

129 

130 

131 C 

132 C 

133 C 

134 

135 

136 

137 C 

138 C 

139 C 

140 

141 C 

142 C 

143 C 

144 

145 

146 

147 


Page 3 
07-14-84 
14:03:20 

Microsoft F0RTRAN77 V3.13 8/05/83 

PX = CC + LL * C 
PY = H + LL * S 


*** move the rotated module out there 

CALL NOTHNG (ROT, 3) 

ROT (1,3) = PX 
ROT (2,3) = PY 
CALL XMUL (ROT, V, 4) 

*** now calculate the shape of the base 
XX = X + CC 

« point E » 

V( 1 , 5) = CC 
V(2,5) = H 

« point F » 

V( 1 ,6) = CC 
V(2,6) = AA 

« point G >> 

V( 1 ,7) = CC 
V(2,7) = 0. 

« point H » 

V( 1 ,8) = -RR 
V(2,8) = 0. 

« point I » 

V ( 1 ,9) = -RR 
V(2,9) = AA 

V(l t 10) = PX 
V(2, 10) = PY 

*** Transform to floor coordinates 

CALL NOTHNG (FLOOR, 3) 

FLOOR (1,3) = X 

CALL XMUL (FLOOR, V, N) 

*** transform to screen coordinates 

CALL XMUL (SDFORM, V, N) 

*** erase old picture 

CALL DRWFLR (VO) 

IF ((EF .EQ. 0) .AND. (EEF .NE. 0)) THEN 
CLR = 0 

CALL SDRAW (VI, N, CLR) 



.) Line// 1 

148 

149 

150 


END IF 
CLR = 1 

CALL SDRAW (V, N, CLR) 



151 

CALL MOVE (V, 

VI, 


152 

EEF * 1 



153 C 




154 

RETURN 



155 

END 


'lame 

Type 

Offset P Class 


\A 

REAL 

198 



REAL*8 

218 


CC 

REAL 

4 /MG 

/ 

CLR 

INTEGER*4 

234 


)D 

REAL 

8 /MG 

/ 

iZEF 

INTEGER*4 

4 /ME 

/ 

EF 

INTEGER*4 

0 /ME 

/ 

r LAC 

INTEGER*4 

0 /MG 

/ 

•LOOR REAL 

158 


11 

REAL*8 

0 * 


▼ r 
\ 

INTEGER*4 

202 



REAL 

12 /MG 

/ 

N 

INTEGER*4 

194 


P 

REAL* 8 

8 * 


’RTFG INTEGER*4 

8 /ME 

/ 

?X 

REAL 

226 


PY 

REAL 

230 


10T 

REAL 

122 


in 

REAL 

16 /MG 

/ 

s 

REAL*8 

210 


CDFORM REAL 

36 /MF 

/ 

fT 

REAL 

24 /MG 

/ 

V 

REAL 

2 


VO 

REAL 

72 /MF 

/ 

'1 

REAL 

192 /MF 

/ 

.Ay 

REAL 

20 /MG 

/ 

X 

REAL*8 

4 * 


:form real 

0 /MF 

/ 
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156 $PAGE 
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PLOT »> 
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157 

158 

159 

C 

C 

SUBROUTINE 

SDRAW (V, N, CLR) 


160 C 

161 C 

162 C 

163 C 

164 C 

165 C This procedure draws the side view of T0M__B 

166 C 

167 C 

168 C 

169 C 

170 C 

171 REAL V(3, 10) 

172 INTEGER N, CLR, XI, X2, Yl, Y2 

173 C 

174 C *** draw mobile base 

175 C 

176 CALL RCT (V, 5, CLR) 

177 C 

178 C *** draw linkage 

179 C 

180 XI = V( 1 ,6) 

181 Yl = V(2,6) 

182 X2 = V( 1 ,5) 

183 Y2 = V(2,5) 

184 CALL LINE (XI, Yl, X2, Y2, CLR) 

185 XI * V( 1 , 10) 

186 Yl = V(2, 10) 

187 CALL LINE (X2, Y2, XI, Yl, CLR) 

188 C 

139 C **- draw mock-up module 

190 C 

191 CALL RCT (V, 0, CLR) 

192 CALL PURGE 

193 CALL CRFRDY 

194 C 

195 CALL HOME 

196 C 

197 RETURN 

198 END 


jme Type 

,R INTEGERS 
INTEGER-4 
REAL 

INTEGER-4 


Offset P Class 

8 * 

4 * 

0 * 

238 


1 



«< 0 M V PLO T >» 


D Line# l 7 
X2 rN'TECER*4 246 

Y1 INTEGERS 242 

Y2 INTEGER *4 250 
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190 SPACE 



0 M v P L 0 T >» 


i^ine# 

200 

201 

202 

203 

204 

205 

206 

207 

208 

209 

210 
211 
212 

213 

214 

215 

216 

217 

218 

219 

220 
221 
222 
223 


1 

C 

C 

c 

c 

c- 

c 

c 

c 

c 

c- 

c 

c 


100 
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SUBROUTINE RCT (V, OFF, CLR) 


This procedure draws a rectangle 


REAL 

INTEGER 


V(3, 10) 

OFF, CLR, X( 10) , Y( 10) 


DO 100 K » 1, 4 
J » K + OFF 
X(K) = V(1,J) 

Y ( K ) = V(2,J) 

CONTINUE 

CALL P0LYGN(4, X, Y, CLR) 

RETURN 

END 


me Type 

.R INTEGERS 

INTEGER -4 
INTECER*4 
■'F INTECF.R*4 

REAL 

INTEGER* 4 
INTEGER*4 


Offset P Class 

8 * 

338 
334 
4 * 

0 * 

254 

294 


224 SPAGE 



C« 0 M V 


) Lino'/ 1 
22 r > 
22b C 

227 C 

228 C 

229 C 

230 C 

231 C 

232 C 

233 C 

234 C 

235 C 

236 C 

237 C- 

238 C 

239 C 

240 

241 

242 

243 

244 C 

245 

246 

247 C 

248 

249 

250 C 

251 

252 

253 

254 

255 

256 

257 

258 

259 

260 
261 

262 C 

263 

264 
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Slll’K’OI IT INK PLOT (CMD) 


This is the plot part of the graphical package, and can 
be directly callable from 0MV or SVX. The value of FLAG 
obtained from the disk file named SIZE.DAT dictates one 
of top or side view to be displayed. 


INTEGER CMD(7) , FUG 

REAL * 8 X, Y, T, UL, UA, H 

REAL XFORM(3,3),SDFORM(3,3),CC,LL,DD,RR,WW,TT 

REAL V0(3, 10), Vl(3, 10) 

COMMON /MG/ FUG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM , SDF0RM, V0, VI 

UL = 10000.0 
UA = UL 


IF (FUC ,EQ. 0) THEN 
T = CMD(l) / UA 

X = CMD(2) / UL 

Y = CMD ( 3 ) / UL 

CALL TOPVEW (X, Y, T) 
ELSE 

!I » CMD(4) / UL 
X = CMD(2) / UL 
T * CMD(5) / UA 
CALL SIDEVEW (H, X, T) 
END IF 

RETURN 

END 


iame 

Type 

Offset P Class 


:c 

REAL 

4 /MG 

/ 

:md 

INTEGER*4 

0 * 


)D 

REAL 

8 /MG 

/ 

7 LAG 

INTEGER*4 

0 /MG 

/ 

1 

REAL*8 

382 


,L 

REAL 

12 /MG 

/ 



0 M v PLOT »> 


inef 1 7 

REAL 
"ORM REAL 
REAL-8 
REAL 
REAL-8 
REALMS 
REAL 
REAL 
REAL 
REAL*8 
ORM REAL 
REAL*8 


16 

/MG 

/ 

36 

358 

/MF 

/ 

24 

350 

342 

/MG 

/ 

72 

/MF 

/ 

192 

/MF 

/ 

20 

366 

/MG 

/ 

0 

374 

/MF 

/ 
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265 SPAGE 


«< o M V P 1. 0 T »> 


D Line# 1 7 

266 C 

267 C 

268 

269 C 

270 C 

271 C 

272 C 

273 C 

274 C 

275 C 

276 C 

277 C 

278 C 

279 

280 
281 
282 

283 

284 C 

285 

286 

287 

288 C 

289 

290 C 

291 C 

292 C 

293 

294 C 

295 C 

296 C 

297 C 

298 C 

299 

300 

301 

302 

303 

304 

305 C 

306 

307 C 

308 C 

309 C 

310 C 

311 

312 

313 

314 
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SUBROUTINE TOPVEW (PX, PY, THETA) 


This procedure constructs the top view of T0M_B. No 
correction to perspective distortion is implemented 


REAL * 8 PX, PY, THETA, S, C 
REAL V(3, 10) , V0(3, 10) , SDFORM(3,3) 

REAL ROTO, 3), FLOOR(3,3), XFORM(3,3) 

REAL CC, DD, LL, RR, WW, TT, Vl(3,10) 

INTEGER FLAG, N, CLR, EF, EEF, PRTFG 

COMMON /MG/ FLAG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM, SDFORM, VO, VI 
COMMON /ME/ EF, EEF, PRTFG 

N = 10 

*** get T0M_B shape at the origin 

CALL ORGPOS (V, N) 

*** rotate by THETA if needed 

IF (THETA .NE. 0.0) THEN 

*** construct rotation matrix 
CALL NOTHNG (ROT, 3) 

CALL SINCOS (THETA, S, C) 


R0T( 1,1) » C 

R0T(1, 2) - -S 

ROT (2,1) - S 

ROT (2,2) - C 


*** rotate it 
CALL XMUL (ROT, V, N) 
END IF 


*** transform to floor coordinates 

CALL NOTHNG (FLOOR, 3) 

FLOOR( 1,3) = PX 
FLOOR(2,3) = PY 
CALL XMUL (FLOOR, V, N) 



0 M V 

PLOT »> 

ine# 1 

7 

315 C 


316 C 

*** transform to screen 

317 C 


318 

CALL XMUL (XF0RM, V, N) 

319 C 


320 C 

*** get ready to draw, 

321 C 


322 

CALL DRWFLR (VI) 

323 

IF ((EF .EQ. 0) .AND. (E 

324 

CLR = 0 

325 

CALL DRAW (VO, N, C 

326 

END IF 

327 C 


328 

CLR = 1 

329 

CALL DRAW (V, N, CLR) 

330 

CALL MOVE (V, VO, N) 

331 

EEF * 1 

332 C 


333 

RETURN 

334 

END 
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^me 

Type 

Offset P 

Class 



REAL*8 

594 


/ 

- 

REAL 

4 

/MG 

R 

INTEGER*4 

602 


/ 

* 

REAL 

8 

/MG 

EF 

INTEGER *4 

4 

/ME 

/ 

-> 

INTEGER*4 

0 

/ME 

/ 

,AG 

INTEGERS 

0 

/MG 

/ 

LOOR 

REAL 

546 


/ 

T ^ 

REAL 

12 

/MG 


INTEGERS 

582 


/ 

rtfc 

INTEGER*4 

8 

/ME 

X 

REAL*8 

0 * 



- 

REAL*8 

4 * 



)T 

REAL 

510 


/ 

R 

REAL 

16 

/MG 


REAL*8 

586 


/ 

)F0RM 

REAL 

36 

/ MF 

IIETA 

REALMS 

8 * 


/ 

T 

REAL 

24 

/MG 


REAL 

390 


/ 

3 

REAL 

72 

/MF 

1 

REAL 

192 

/MF 

/ 

A 

REAL 

20 

/MG 

/ 

FORM 

REAL 

0 

/MF 

/ 


335 SPAGE 
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336 C 

337 C 


338 SUBROUTINE MOVE (V, VO, N) 

339 C 

340 C 

341 C 

342 C 

343 C 

344 C This procedure saves the shape vector V 

345 C 

346 C 

347 C 

348 C 

349 C 

350 REAL V(3, 10) , V0(3,10) 

351 C 

352 DO 100 K - 1, N 

1 353 DO 100 J = 1, 3 

2 354 V0(J,K) = V(J,K) 

2 355 100 CONTINUE 

356 C 

357 RETURN 

358 END 


Name Type 

J INTECER*4 

K INTEGERS 

n INTEGER *4 

/ REAL 

VO REAL 


Offset P Class 

614 
606 
8 * 

0 * 

4 * 


359 SPAGE 



0 M V PLOT »> 


Lne" 1 

360 C 

361 C 

362 

363 C 

364 C 

365 C— 

366 C 

367 C 

368 C 

369 C 

370 C 

371 C 

372 C— 

373 C 

374 C 

375 

376 C 

377 

378 

379 

380 200 

381 

382 100 

383 C 

384 

385 
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SUBROUTINE NOTHNG (A, N) 


This procedure initializes an N x N matrix A to a 
unit matrix 


REAL A ( N , N ) 

DO 100 K - 1, N 
DO 200 J « 1, N 
A(K,J) - 0.0 
CONTINUE 
A(K,K) = 1.0 
CONTINUE 

RETURN 

END 


t me Type Offset P Class 

REAL 0 * 

INTECF.P.*4 626 

INTEGER* 4 618 

INTECER*4 4 * 


386 SPACE 



«< 0 M V 


D Line# 1 

387 C 

388 0 

389 

390 C 

391 C 

392 C 

393 C 

394 C 

395 C 

396 C 

397 C 

398 C 

399 C 

400 C 

401 C 

402 

403 

404 C 

405 

1 406 

2 407 

2 408 

3 409 

3 410 300 

2 411 

2 412 200 

1 413 

2 414 

2 415 400 

l 416 100 

417 C 

418 

419 
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SUBROUTINE XMUL (R, V, N) 


This procedure uses a transformation matrix R and 
transforms the shape vector V having N columns 


REAL R(3,3) , V(3,10), T(3), S 

INTEGER ROW, COL 

DO 100 COL = 1, N 
DO 200 ROW =1,3 
S = 0.0 

DO 300 J - 1, 3 

S = S + R(R0W, J) * V(J, COL) 
CONTINUE 
T(ROW) = S 
CONTINUE 
DO 400 L = 1, 3 
V(L,C0L) = T(L) 

CONTINUE 

CONTINUE 

RETURN 

END 


Name 

Type 

Offset 

30L 

INTEGER* 4 

646 

J 

INTEGERS 

662 


INTEGERS 

666 


INTEGERS 

8 


REAL 

0 

’OW 

INTEGERS 

654 


REAL 

658 


REAL 

634 

J 

REAL 

4 


P Class 


* 

* 


* 


ORIGINAL PAGE IS 
©E POOR QUALITY 


420 $PAGE 



0 


ine# 

421 

422 

423 

424 

425 

426 

427 

428 

429 

430 

431 

432 

433 

434 

435 

436 

437 

438 

439 

440 

441 

442 

443 
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C 

c 

SUBROUTINE ORGPOS (V, N) 

C 

C 

C 

C 

C This procedure calculates the shape vector V of T0M_B 

C at the orgin. Only the top view is considered here. 

C 

C 



c 

c 

REAL V(3, 10) , XF0RM(3,3) , V0(3,10), W(2) 

REAL VI (3, 10) 

REAL CC, DD, LL, RR, WW, CL, SDFORM(3,3) 

INTEGER FLAG, CORNR(2,2), EF, EEF , PRTFG 

C 

COMMON /MG/ FLAG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM , SDFORM, VO, VI 
COMMON /ME/ EF, EEF, PRTFG 


444 C 

445 C 

446 DO 100 K - 1, N 

447 V(3, K) = 1.0 

448 100 CONTINUE 

449 C 

450 C *** set up shape matrix V 

451 C 


452 

CL 

CC + LL 




453 C 




Corner 

« 

A » 

454 

V(l, 1) 

= 

CC 




455 

V ( 2 , 1) 

= 

0 




456 C 




Corner 

« 

B » 

457 

V(l, 2) 

S 

CC 




458 

V(2, 2) 

= 

-WW 




459 C 




Corner 

« 

C » 

460 

V(l, 3) 

s 

-RR 




461 

V(2, 3) 

s 

-WW 




462 C 




Corner 

« 

D » 

463 

V(l, 4) 

as 

-RR 




464 

V(2, 4) 

= 

WW 




465 C 




Corner 

« 

E » 

466 

V(l, 5) 

s 

CC 




467 

V(2, 5) 

as 

WW 




468 C 




Corner 

« 

MM » 

469 

V(l, 6) 

= 

CL 






«< 0 M V P 

LOT »> 


D Line" 1 7 




470 

V(2 , 6) 

= 0 


471 C 




472 

V(l, 7) 

= CL + 

TT 

473 

V(2, 7) 

= -DD 


474 C 




475 

V(l, 8) 

- CL - 

TT 

476 

V(2, 8) 

= -DD 


477 C 




478 

V(l, 9) 

=* CL - 

TT 

479 

V(2, 9) 

= DD 


480 C 




481 

V(1 , 10) 

* CL + TT 

482 

V(2, 10) 

= DD 


483 C 




484 

RETURN 



485 

END 



Name Type 

Offset 

P Class 


CC REAL 

4 

/MG 

/ 

CL REAL 

702 



CORNR INTEGERS 

678 



DD REAL 

8 

/MG 

/ 

EEF INTECER*4 

4 

/ME 

/ 

EF INTEGER*4 

0 

/ME 

/ 

FLAG INTEGERS 

0 

/MG 

/ 

K INTEGER*4 

694 



LL REAL 

12 

/MG 

/ 

N IMTEGER*4 

4 

* 


PRTFG INTEGER-4 

8 

/ME 

/ 

RR REAL 

16 

/MG 

/ 

SDFORM REAL 

36 

/MF 

/ 

IT REAL 

24 

/MG 

/ 

V REAL 

0 = 

* 


VO REAL 

72 

/MF 

/ 

/I REAL 

192 

/MF 

/ 

REAL 

670 



WW REAL 

20 

/MG 

/ 

(FORM REAL 

0 

/MF 

/ 
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Corner << F » 


Corner « G >> 


Corner « H >> 


Corner << I >> 


ORIGINAL PAGE IS 
C£ POOR QUALITY 


486 SPAGE 



0 M V 


PLOT »> 


'A 


Anctt 1 

487 C 

488 C 

489 

490 C 

491 C 

492 C— 

493 C 

494 C 

495 C 

496 C 

497 C 

498 C 

499 C 

500 C— 

501 C 

502 C 

503 

504 

505 C 

506 

507 C 

508 

509 

510 

511 C 

512 

513 

514 

515 

516 

517 200 
513 

519 

520 

521 C 

522 

523 

524 300 

525 

526 

527 

528 C 

529 

530 

531 

532 

533 

534 C 

535 C 
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SUBROUTINE INITPL 


This procedure initializes the system and calculates 
all the necessary transformation matrices based on 
the data obtained from SIZE.DAT 


REAL V0(3,10),XF0RM(3,3),SDF0RM(3,3), W(2) 

REAL CC, DD, LL, RR, WW, TT, Vl(3,10) 

REAL C0RNR(2,2) , W(2) 

INTEGER FLAG, EF, C0RNR(2,2), EEF, CORNS (2, 2), PRTFG 

COMMON /MG/ FLAG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM , SDFORM, VO, VI 
COMMON /ME/ EF, EEF, PRTFG 

EEF = 0 

OPEN (7, FILE = ’SIZE.DAT’) 

READ (7, 10) CC, DD, LL, RR, WW, TT 
DO 200 K = 1, 2 

READ (7, 20) (C0RNR(K, J) , J-l, 2) 

CONTINUE 
W(l) = 12.2 
W(2) = 24.4 

CALL CORDX ( CORNR , XFORM, W) 

DO 300 K = 1, 2 

READ (7, 20) (CORNS(K, J) , J-1,2) 

CONTINUE 
W(l) * 12.2 
W(2) = 6.096 

CALL CORDX (CORNS, SDFORM, W) 

READ (7,20) EF 
READ (7, 20) FLAG 
READ (7, 20) PRTFG 
CLOSE (7) 

FLG * 1 


*** calculate floor shape 
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536 C 


537 

JW = 30 

538 

JL = 44 

539 C 


540 

IF (FLAG .EQ. 0) THEN 

541 

J1 = C0RNR(1,1) 

542 

LI = C0RNR( 1,2) 

543 

J2 = C0RNR(2, 1) 

544 

L2 = C0RNR(2,2) 

545 

JJ = (L2 - LI + 1) 

546 

Vl(l,l) = Ji 

547 

Vl( 2, 1 ) = LI 

548 

V 1 ( 1,2) = J2 

549 

Vl(2,2) = LI 

550 

Vl( 1 ,3) = J2 

551 

Vl(2,3) = L2 

552 

Vl( 1 ,4) = Jl 

553 

Vl(2,4) = L2 

554 

VI ( 1 , 5) = Jl 

555 

Vl(2, 5) « L2 + JW - JJ 

556 

Vl( 1 ,6) = Jl - JL 

557 

VI (2,6) = L2 + JW - JJ 

558 

Vl( 1 , 7) = Jl - JL 

559 

Vl(2,7) = L2 - JL - JJ 

560 

Vl( 1 ,8) = Jl 

561 

Vl(2,8) = L2 - JL - JJ 

562 

Vl( 1 ,9) = -1000.0 

563 

Vl(2, 9) = -1000.0 

564 

ELSE 

565 

Jl = C0RNS( 1,1) 

566 

LI = CORNS ( 1,2) 

567 

J2 = CORNS(2,l) 

568 

L2 = CORNS(2,2) 

569 

V0(1,1) = Jl - JL 

570 

V0(2, 1 ) = L2 + 1 

571 

V0( 1,2) = J2 + JL 

572 

V0(2, 2) = L2 + 1 

573 

V0( 1,3) = -1000.0 

574 

VO(2,3) = -1000.0 

575 

END IF 

576 C 


577 

CALL GRAFICS 

578 

RETURN 

579 10 

FORMAT (FI 5. 8) 

580 20 

FORMAT (13) 

581 

END 


ime Type 


Offset P Class 
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cc 

REAL 

4 

/MG 

/ 

ORNR 

INTEGERS 

714 



ORNS 

INTEGERS 

730 



DD 

REAL 

8 

/MG 

/ 

~EF 

INTEGERS 

4 

/ME 

/ 

F 

INTEGER*4 

0 

/ME 

/ 

FLAG 

INTEGERS 

0 

/MG 

/ 

"LG 

REAL 

758 




INTEGERS 

750 



^1 

INTEGER*4 

770 



J2 

INTEGER*4 

778 



J 

INTEGERS 

786 



L 

INTEGER*4 

766 



JW 

INTEGER*4 

762 



' r 

INTEGER*4 

746 



1 

INTEGER*4 

774 



L2 

INTEGER *4 

782 



: L 

REAL 

12 

/MG 

/ 

RTFG 

INTEGER*4 

8 

/ME 

/ 

vR 

REAL 

16 

/MG 

/ 

3DF0RM 

REAL 

36 

/MF 

/ 

T 

REAL 

24 

/MG 

/ 

0 

REAL 

72 

/MF 

/ 

/I 

REAL 

192 

/MF 

/ 


REAL 

706 



w 

REAL 

20 

/MG 

/ 

.FORM 

REAL 

0 

/MF 

/ 


582 SPAGE 


ORIGINAL PAGE IS 
OF! POOR QUALITY 



0 M V 


PLOT »> 


-in e// 1 
533 C 

584 C 

585 

586 C 

587 C 

588 C— 

589 C 

590 C 

591 C 

592 C 

593 C— 

594 C 

595 C 

596 

597 

598 C 

599 

600 C 

601 C 

602 100 

603 

604 

605 

606 

607 C 

608 C 

609 

610 
611 
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SUBROUTINE DRWFLR (V) 


This subroutine draws the floor portion of graphics 


REAL V(3, 10) 

INTEGER CT, X(10), Y(10) 

CT = 1 

REPEAT 
K * CT 

X ( K ) = V( 1 ,K) 

Y(K) - V(2,K) 

CT * CT + 1 

IF ( V( 1 ,CT) .GE. -100.0) GO TO 100 
UNTIL V( 1 ,CT) < -100.0 

CALL POLYGN (X , X, Y, 1) 

RETURN 

END 


ame Type 

I NT EGER* 4 
INTEGER*4 
REAL 

INTEGER*4 

INTEGER*4 


Offset P Class 

884 
888 
0 * 

804 

844 


612 SPACE 
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1 7 


INTEGERS 

892 

INTEGERS 

900 

L\'TEGER*4 

896 

INTEGER*4 

904 
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656 SPACE 


C« 0 M V PLOT »> 


) Line if 1 

657 C 

658 C 

659 

660 C 

661 C 

662 C- 

663 C 

664 C 

665 C 

666 C 

667 C 

668 C 

669 C- 

670 C 

671 C 

672 

673 

674 C 

675 C 

676 C 

677 

678 

679 
630 C 
681 
682 
633 

684 C 

685 

686 

687 

688 C 
639 
690 
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SUBROUTINE CORDX (C, T, W) 


This procedure computes the necessary transformation 
matrices from floor to screen coordinates 


INTEGER C(2,2) 

REAL T(3,3) , W(2) 

*** set up transformation matrix T 

T( 1 ,3) = C(l,l) 

T(2,3) = C(2,2) 

T(3,3) = 1.0 

T( 1 , 1) » (C(2,l) - T(l,3)) / W(l) 

T(2, 1 ) = (C(2,2) - T(2,3)) / W(l) 

T(3, 1 ) = 0.0 

T( 1 1 2) = (C(l, 1) - T(l, 3)) / W(2) 

T(2, 2) = (C( 1,2) - T(2,3)) / W(2) 

T(3,2) = 0.0 

RETURN 

END 


ime Type Offset P Class 

INTEGER -4 0 * 

REAL 4 * 

REAL 8 * 


691 SPACE 


<. 0 M V 


..ine# 1 

692 C 

693 C 

694 C 

695 C 

696 C 

697 C 

698 C 

699 

700 C 

701 C 

702 C 

703 C 

704 

705 

706 

707 

708 

709 
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This is a graphics package for the TECMAR GRAPHICS MASTER board 
written under Microsoft's FORTRAN 77. To use this package, one 
must include this package in the source file. A graphics master 
must already be installed, or the software will hang. 


SUBROUTINE PURGE 

This procedure purges the graphics buffer and forces the board 
to complete the drawing by closing the graphics channel. 

INTEGER GRF 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 
CLOSE (GRF) 

RETURN 

END 


le Type 


Offset P Class 


CHAR*1 4 /GMBD / 

INTEGERS 0 /GMBD / 


710 $PAGE 
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711 C 


712 C 

713 SUBROUTINE GRFRDY 

714 C 

715 C This procedure opens the graphics channel and sets it ready for 

716 C communication 

717 C 

718 C 

719 INTEGER GRF 

720 CHARACTER ESC 

721 COMMON /CMBD/ GRF, ESC 

722 OPEN (GRF, FILE = ’gm’) 

723 RETURN 

724 END 


Name 

Type 

Offset P Class 


ESC 

CHAR* 1 

4 

/GMBD 

/ 

GRF 

INTEGERS 

0 

/GMBD 

/ 


725 $PAGE 
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,ine« 1 

726 C 

727 C, 

728 

729 C 

730 C 

731 C 

732 C 

733 C 

734 C 

735 

736 

737 

738 

739 

740 10 

741 


SUBROUTINE SETFB (FG, BG) 

This procedure sets the foreground color to FG and the backgroun 
color to BG. Both arguments must be of INTEGER type. 


INTEGER GRF, FG, BG 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 
WRITE (GRF, 10) ESC, FG, BG 
RETURN 

FORMAT (' ’, Al, 12, 12, ’c'\) 

END 


ame Type 

INTEGERS 
SC CHAR*1 

r: INTEGERS 

F INTEGERS 


Offset P Class 

*r» 

4 /GMBD / 

0 * 

0 /GMBD / 


742 $PAGE 


/« 0 M V 


D Line# 1 

743 C 

744 C 

745 

746 C 

747 C 

748 C 

749 C 

750 C 

751 C 

752 

753 

754 

755 C 

756 

757 

758 

759 

760 

761 C 

762 

763 

764 

765 C 

766 10 

767 20 

768 30 

769 
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SUBROUTINE GRAFICS 

This procedure enters the GM graphics mode with a four-line text 
window at the bottom 


INTEGER GRF 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 


GRF = 9 

ESC = CHAR(27) 

CALL GRFRDY 

WRITE (GRF, 10) ESC 
WRITE (GRF, 20) ESC 
WRITE (GRF, 30) ESC 
CALL SETFB (1,0) 
CALL HOME 
RETURN 


FORMAT (' ', 
FORMAT (' ', 
FORMAT (' ', 
END 


Al, ' ( !0m'\) 

Al, «[!640;352;2g , \) 
Al, *[21;24r’\) 


Name Type 
CHAR 

ESC CHAR*1 

GRF INTEGER*4 


Offset P Class 

INTRINSIC 
4 /GMBD / 

0 /GMBD / 


770 $PAGE 


-inc# 1 

771 C 

772 C 

773 

774 C 

775 C 

776 C 

777 C 

778 C 

779 C 

780 

781 

782 

783 C 

784 

785 

786 

787 

788 

789 

790 

791 10 

792 30 

793 
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SUBROUTINE QUITGM 


This procedure gets one out of graphics mode and returns 
to text mode 


CHARACTER CH, ESC 

INTEGER GRF 

COMMON /GMBD/ GRF, ESC 

CALL HOME 
WRITE (GRF, 30) 

CALL PURGE 
READ (*, 10) CH 
CALL GRFRDY 
CALL TEXT 
RETURN 
FORMAT (Al) 

FORMAT ('Press <CR> to continue ... '\) 
END 


Type 


Offset P Class 


CHAR* 1 1015 

CHAR*1 4 /GMBD / 

INTEGER *4 0 /GMBD / 


794 $PAGE 


«< 


0 M V PLOT »> 


D Line# 1 

795 C 

796 c 

797 

798 C 

799 C 

500 C 

501 C 
802 C 
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SUBROUTINE TEXT 

This procedure returns the system to text mode 



803 

INTEGER 

GRF 


804 

CHARACTER 

ESC 


305 

COMMON /CMBD/ 

GRF, ESC 


806 
807 
308 C 

WRITE (GRF, 10) ESC 
RETURN 


309 10 
810 

FORMAT (’ Al, 

END 

'[ !80;25;la'\) 

Name 

' Type 

Offset P Class 


ESC 

CHAR*1 

4 /GMBD 

/ 

GRF 

INTEGERS 

0 /GMBD 

/ 


811 $PAGE 


< o :i v 


^ine-, ! 1 
812 C 
3 1 3 C 

814 

815 C 

816 C 

817 C 

818 C 

819 C 

820 
321 
822 

823 

824 10 

825 
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SUBROUTINE LINE (XI, Yl, X2, Y2, COLOR) 


This procedure draws a line from (XI, Yl) to (X2,Y2) in COLOR 


INTEGER GRF, XI, Yl, X2, Y2, COLOR 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 

WRITE (GRF, 10) ESC, XI, Yl, X2, Y2, COLOR 

FORMAT (’ *, Al, ’[!’, 4(13, V), 13, * 1 * \) 

END 


le Type 

LOR INTEGER-4 
: CiIAR-1 

INTEGER *4 
INTECER*4 
INTEGER-4 
INTEGER*4 
INTEGER-4 


Offset P Class 
16 * 

4 /GMBD 
0 /GMBD 
0 - 
8 * 

/ j 

12 * 


/ 

/ 


026 SPAGE 
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827 C 
328 C 

829 SUBROUTINE HIDELN (XI, Yl, X2, Y2, COLOR) 

330 C 
831 C 

332 C This procedure draws the line (XI, Yl) - (X2,Y2) but aborts drawi 

833 C before reaching target if a dot in a color other that BG is 

834 C encountered 

835 C 

836 C 

337 INTEGER GRF, XI, Yl, X2, Y2, COLOR 

830 CHARACTER ESC 

839 COMMON /GMBD/ GRF, ESC 

340 WRITE (GRF, 10) ESC, XI, Yl, X2, Y2, COLOR 

341 RETURN 

342 10 FORMAT (* ', Al, 4(13, ';'), 13, ’S'\) 

343 END 


ime 

Type 

Offset 

P 

Class 

•-0L0R 

INTEGER*4 

16 

-r 


9C 

CIIAR-1 

4 


/GMBD 

RF 

INTEGER-4 

0 


/GMBD 

:i 

INTEGERS 

0 

* 



INTEGER-4 

8 

£ 


1 

INTEGER-4 

4 

u. 

*r» 


9 

INTEGER*4 

12 

V 



844 SPAGE 



0 M V 
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845 C 

846 C 

847 SUBROUTINE POLYGN (N, X, Y, COLOR) 

848 C 

849 C 

85() C This procedure draws a closed polygon whose N vertices are store 

851 C in the arrays X and Y. The color to be used is COLOR 

852 C 

853 C 

854 INTEGER GRF, X(N), Y(N) , COLOR 

855 CHARACTER ESC 

856 COMMON /GMBD/ GRF, ESC 

857 WRITE (GRF, 10) ESC 

858 DO 100 K = 1, N 

859 WRITE (GRF, 20) X(K), Y(K) 

860 100 CONTINUE 

361 WRITE (GRF, 30) COLOR 

862 RETURN 

863 10 FORMAT (' ' , Al, '[!’\) 

864 20 FORMAT ( 2(13, ’;')\) 

865 30 FORMAT ( 13, 'p'\) 

866 END 


me Type 

;„0R INTEGER*4 
C CHAR-1 

F INTEGER-4 

INTEGERS 
INTEGER *4 
INTEGERS 
INTEGER-4 


Offset P Class 
12 * 

4 /GMBD 
0 /GMBD 
1160 
0 * 

4 * 

8 * 


/ 

/ 


867 SPACE 


ORIGINAL PAGE IS 
OF POOR QUALITY 


'« 0 


M V 


D Line;/ 1 

868 C 

869 C 

870 

871 C 

872 C 

873 C 

874 C 

875 C 

876 

877 

878 C 

879 

880 C 

881 
882 

883 10 

884 
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SUBROUTINE HOME 

THIS SUBROUTINE HOMES THE CURSOR 


INTEGER GRF 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 

WRITE (GRF, 10) ESC 
RETURN 

FORMAT (' ', Al, ’[ 1 ; 1 f'\) 
END 


Name Type 

ESC CHAR*1 

GRF INTEGER*4 


Offset P Class 

4 /GMBD / 

0 /GMBD / 


885 SPACE 



0 :i V P L 0 T >>> 


uLnoff 1 
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Type 


Size Class 


)RDX 

■•w 

FLR 

;bd 

’AFIC 

RDY 

.^ELN 

)ME 

TPL 

E 


n'E 

•TUNG 

IPOS 

.JT 

)LYGN 

ICE 

:tgm 

:t 

'RAW 

r fb 

DEVE 

NCOS 

KT 

PVEW 

1UL 


SUBROUTINE 
SUBROUTINE 
SUBROUTINE 
5 COMMON 

SUBROUTINE 
SUBROUTINE 
SUBROUTINE 
SUBROUTINE 
SUBROUTINE 
SUBROUTINE 
12 COMMON 
312 COMMON 
28 COMMON 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 


'ass One No Errors Detected 
885 Source Lines 


ORIGINAL’ PAGE IS 
OF POOR QUALITY 


APPENDIX 3 


OMV Data Files Used During Development 



File : INITCON.DAT 


0.0 

0.0 

0.0 

0.00 

0.0 

0.0 

0.0 

0.0 

0.0 


This file contains all the needed initial conditions 


P0S(1) — initial condition 

P0S(2) — initial condition 

P0S(3) — initial condition 

VEL(l) — initial condition 

VEL(2) — initial condition 

VEL(3) — initial condition 

EUL(l) — initial condition .. ROLL 

EUL(2) — initial condition .. PITCH 

EUL(3) — initial condition .. YAW 



File : MDLPRM.DAT 


This file contains all the model parameters needed by OMV 


00.075 

00.075 

00.075 

000.52359878 

000.52359878 

000.52359878 

7048.37 

3713.95 

3713.95 

3282.75 

0.1 

1 

10 

200.0 


ACC(l) : Acc along X-axis (body) 

ACC(2) : Acc along Y-axis (body) 

ACC(3) : Acc along Z-axis (body) 

WWB(l) : body rate about X axis 

WWB(2) : body rate about Y axis 

WWB(3) : body rate about Z axis 

1X1(1) principal moment of inertia along 1 axis 

III ( 2 ) principal moment of inertia along 2 axis 

II I ( 3 ) principal moment of inertia along 3 axis 

Mass in kilograms 

major cycle period in seconds 

MODE : 1 for position control 

No. of steps per major cycle 

altitude of orbit in kilo-meters 


File : SVXINT.DAT 


This file contains all the system initialization data needed by the SVX module 


0.5588 

CC 

IN METERS 

0.762 

LL 

IN METERS 

11.668 

AA 

IN METERS 

2.4384 

HH 

IN METERS 

7048.37 

IINV(l) 

3713.95 

IINV(2) 

3713.95 

IINV(3) 


File : HNDSGL.DAT 


This file contains the simulated hand controller signals 

(Partial list) 


100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 


File : SIZF.DAT 


This file contains all the plot parameters 
for the 

graphics package PLOT 


0.5588 

2.1336 

0.762 

1.016 

0.6096 

0.3048 

409 

001 

630 

350 

100 

152 

500 

300 

000 

000 

001 


CC : 22 inches 

DD : 84 inches 

LL : 30 inches 

RR : 40 inches 

WW : 24 inches 

TT : 12 inches 

CORNR(l.l) 

C0RNR( 1,2) 

C0RNR(2, 1) 

CORNR(2,2) 

CORNR(l.l) SIDE VIEW 

C0RNR( 1,2) SIDE VIEW 

C0RNR(2, 1) SIDE VIEW 

C0RNR(2,2) SIDE VIEW 

PLOT MODE : <> 0 MEANS NO CLEAR 

VIEW : 0 = TOP VIEW, <> 0 = SIDE VIEW 

PRTFG : 1-PLOT 2-PRINT 3-PLOT & PRINT 




APPENDIX 4 


OMV Mathematical Model (OMM) Source Listing 



And} 

1 

2 

3 

4 

5 

6 

7 

8 
9 

10 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 

43 

44 

45 

46 

47 

48 

49 
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$LINES1ZE:79 

5PAGESIZE: 56 

STITLE: '<« 0 M V »>' 

C 

C OMV SIMULATION MODEL 

C 

c 

C by 

C 

c 

C Dr. W. Teoh 

C 

C UAH Huntsville 

C 1984 

C 




c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c- 

c 


c 

c 


This is a simplified version of a mathematical simulation 
model of the OMV. In this model, the following simplfications 
and assumptions are made : 

1. The hand controllers provide signals that are interpreted 
as a force at the center of mass and/or a torque about the 
center of mass to provide a rotation of constant angular 
velocity. 

2. The target vehicle is in a circular orbit; the altitude of 
this orbit is inputted from the MDLPRM.DAT file. 

3. Orbital mechanics is implemented, but smaller perturbation 
effects are totally ignored. 

4. Detailed placement of thrusters is not considered (Please 
see assumption 1. above) 

5. Roll, pitch and yaw denote the instantaneous orientation 
of the OMV. 

A 14 component state vector is generated by this model, and 
this state vector serves as input to the SVX module. 


REAL * 8 
REAL * 8 
REAL * 8 
INTEGER 
INTEGER * 4 


X ( 3 ) , V(3), E(3) , A(3) , W(3), Q( 4) 
P0S(3) , VEL(3) , EUL(3) , OMEGA 
111(3), S( 14) , MASS, CYCLE 
CMD(7) , IN, FLAG, MODE, STEP 
TIME 


COMMON /MC/ III, MASS, CYCLE, MODE, STEP 
COMMON /PC/ P0S, VEL, EUL, OMEGA 


system initialization 


<< 0 M V 


Line# 1 

50 C 

51 C 

52 

53 

54 

55 

56 C 

57 C 

58 C 

59 C 

60 

61 C 

62 C 

63 C 

64 C 

65 

66 

67 

68 

69 

70 

71 

72 C 

73 C 

74 C 

75 C 

76 100 

77 C 
73 C 

79 C 

80 C 

81 

82 C 

83 C 

84 C 

85 

86 C 

87 C 

88 C 

89 C 

90 

91 

92 

93 C 

94 C 

95 C 

96 

97 

98 C 


>>> 

7 
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IN = 2 

TIME = -1 

CALL OMVMDL (IN) 

OPEN (IN, FILE = 'HNDSGL.DAT') 

*** *** Note : this invokes graphics routines, and can be 

elimiated if no graphics output. 

CALL INITPL 

*** calculate the initial quaternions at the start of the 
*** simulation and read hand controller 

CALL DETQ (EUL, Q) 

CALL HNDCTL (IN, FLAG, A, W) 

CALL MATCH (EUL, POS, VEL, E, X, V, 3) 

CALL STATE (Q, S, W) 

CALL SVX (S, CMD, MODE) 

CALL OUTPUT (A, W, X, V, E, Q, S, CMD, TIME) 

TIME = 0 

*** main processing loop 

WHILE (FLAG = 0) DO 

IF (FLAG .NE. 0) GOTO 900 

*** copy initial state into work vectors and use these 
*** work vectors for solving the equations of motion 

CALL MOTION (X, V, E, A, W, Q) 

*** update dynamic state 

CALL MATCH (E, X, V, EUL, POS, VEL, 3) 

*** calculate state vector and pass it on to the State 
*** Vector Transformation module 

CALL STATE (Q, S, W) 

CALL SVX (S, CMD, MODE) 

CALL OUTPUT (A, W, X, V, E, Q, S, CMD, TIME) 

*** poll hand controller and get the next set of signals 

CALL HNDCTL (IN, FLAG, A, W) 

GOTO 100 
END WHILE 



Line# 1 
99 900 
100 

101 C 

102 C 

103 C 

104 

105 C 

106 C 

107 C 

108 
109 


12:51:14 

Microsoft FORTRAN77 V3.13 8/05/83 

CONTINUE 
CLOSE (IN) 

*** **# This is also a call to the graphics package 
CALL QUITGM 

*** Grand exit, stage left 

STOP 

END 


me 

Type 

Offset 

P Class 



REAL*8 

242 



3 

INTEGER*4 

266 



CLE 

REAL*8 

32 

/MC 

/ 


REAL*8 

74 




REAL*8 

48 

/PC 

/ 

^AG 

INTEGER*4 

302 



I 

REAL*8 

0 

/MC 

/ 


INTEGER*4 

294 



>SS 

REAL*8 

24 

/MC 

/ 

>DE 

INTEGER*4 

40 

/MC 

/ 

EGA 

REAL*8 

72 

/PC 

/ 

S 

REAL*8 

0 

/PC 

/ 


REAL*8 

98 




REAL*8 

130 



EP 

INTEGER*4 

44 

/MC 

/ 

.ME 

INTEGER*4 

298 




REAL*8 

26 



:l 

REAL*8 

24 

/PC 

/ 


REAL*8 

50 




REAL*8 

2 




110 SPAGE 


«< 0 M V »> 
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111 C 

112 C 

113 SUBROUTINE OMVMDL (IN) 

114 C 

115 C 

116 C 

117 C This procedure obtains the necessary parameters of the OMV 

118 C by reading them from a disk file called MDLPRM.DAT after 

119 C getting the initial state of the OMV (from a file called 

120 C INTTC0N.DAT 

121 C 

122 C 

123 C 

124 REAL * 3 P0S(3), VEL(3), EUL(3), OMEGA 

125 REAL * 8 ACC(3), 111(3), WWB(3), INV(3) 

126 REAL * 8 MASS, CYCLE, ORBIT 

127 INTEGER IN, MODE, STEP 

128 C 

129 COMMON /DC/ ACC, WWB 

130 COMMON /MC/ III, MASS, CYCLE, MODE, STEP 

131 COMMON /PC/ POS, VEL, EUL, OMEGA 

132 C 


133 C 


get initial conditions of the OMV 

134 C 



135 

OPEN 

(IN, FILE = 'INITCON.DAT' ) 

136 

CALL 

VECTOR (IN, POS, 3) 

137 

CALL 

VECTOR (IN, VEL, 3) 

138 

CALL 

VECTOR (IN, EUL, 3) 

139 

CLOSE (IN) 

140 C 



141 C 

*** 

read acceleration, angular rates and 

142 C 


principal moments of inertia in body frame 

143 C 



144 

OPEN 

(IN, FILE = 'MDLPRM.DAT') 

145 

CALL 

VECTOR (IN, ACC, 3) 

146 

CALL 

VECTOR (IN, WWB, 3) 

147 

CALL 

VECTOR (IN, III, 3) 

148 C 



149 C 


read mass characteristics & other parameters 

150 C 



151 

READ 

(IN, 10) MASS 

152 

READ 

(IN, 10) CYCLE 

153 

READ 

(IN, 20) MODE 

154 

READ 

(IN, 30) STEP 

155 

READ 

(IN, 10) ORBIT 

156 

CLOSE 

(IN) 


157 C 

158 C 

159 C 


*** calculate orbital frequency 


« 0 H V »> 
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160 CALL ANCFRE (ORBIT, OMEGA) 

161 C 

162 C 

163 RETURN 

164 10 FORMAT (F15.8) 

165 20 FORMAT (II) 

166 30 FORMAT (12) 

167 END 


ame Type Offset P Class 


V 

REALMS 

0 

/DC 

/ 

.CLE 

REAL*8 

32 

/MC 

/ 

UL 

REAL*8 

48 

/PC 

/ 

I 

REAL*8 

0 

/MC 

/ 


INTEGER*4 

0 * 



\'V 

REAL*8 

306 



*SS 

REAL*8 

24 

/MC 

/ 

DE 

INTEGER*4 

40 

/MC 

/ 

nEGA 

REAL*8 

72 

/PC 

/ 

RBIT 

REAL*8 

330 



S 

REAL* 8 

0 

/PC 

/ 

.EP 

INTEGER*4 

44 

/MC 

/ 

EL 

REAL*8 

24 

/PC 

/ 

B 

REAL*8 

24 

/DC 

/ 


168 $PAGE 
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169 C 

170 C 

171 SUBROUTINE ANGFRE(ORB, W) 

172 C 

173 C 

174 C 

175 C 

176 C This procedure calculates the orbital angular frequency 

177 C at a given altitude. In this calculation, the altitude 

178 C must be given in kilo-meters. This is necessary in order 

179 C for the calculations to be carried out without lossing 

180 C precision. The angular frequency W is in rad/second 

181 C 

182 C 

183 C 

184 REAL * 8 ORB 

185 REAL * 8 ALT, R3, W 

186 C 

187 ALT = ORB * 0.001 

188 R3 = (6.370 + ALT) ** 3 

189 W - DSQRT (398.86 / R3) * 0.001 

190 RETURN 

191 END 


Name 

Type 

Offset 

P 

Class 

ALT 

REAL*8 

358 



DSQRT 

ORB 

REAL*8 

0 

* 

INTRINSIC 

R3 

REALMS 

366 



W 

REAL*8 

4 

♦ 



192 SPACE 


0 M V »> 
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193 C 

194 C 

195 SUBROUTINE VECTOR (M, A, N) 

196 C 

197 C 

198 

199 C 

200 C 

201 C This procedure reads a vector A of N elements from input 

202 C unit M 

203 C 

204 

205 C 

206 INTEGER M, N 

207 REAL * 8 A(N) 

208 C 

209 DO 100 K - 1, N 

210 READ (M, 10) A(K) 

211 100 CONTINUE 

212 RETURN 

213 10 FORMAT (F15.8) 

214 END 

le Type Offset P Class 

REAL*8 4 * 

INTEGERS 374 

INTEGER*4 0 * 

INTEGERS 8 * 


215 $PAGE 
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216 C 

217 C 

218 SUBROUTINE HNDCTL (IN, FLAG, A, W) 

219 C 

220 C 

221 C 

222 C Simulates hand controllers input by reading from a file 

223 C (called HNDSGL.DAT 12) integers to simulate a 12 bit output 

224 C of the hand controllers. Bit assignments are as follows : 

225 C 


226 C 

227 C 

bit 

meaning (direction 

sssssszsssssssssss: 

in 

body frame) 

228 C 

1 

Accelerate along 

+i 

axis 

229 C 

2 

Accelerate along 

-l 

axis 

230 C 

3 

Accelerate along 

+2 

axis 

231 C 

4 

Accelerate along 

-2 

axis 

232 C 

5 

Accelerate along 

+3 

axis 

233 C 

6 

Accelerate along 

-3 

axis 

234 C 

7 

Rotate about 

+1 

axis 

235 C 

8 

Rotate about 

-1 

axis 

236 C 

9 

Rotate about 

+2 

axis 

237 C 

10 

Rotate about 

-2 

axis 

238 C 

11 

Rotate about 

+3 

axis 

239 C 

12 

Rotate about 

-3 

axis 


240 C 

241 0 

242 C 


243 

REAL * 8 

ACC(3), 

WWB(3) 

244 

REAL * 8 

A(3) , 

W(3) 

245 

INTEGER 

SL(6), 

SA(6) , FLAG 

246 

COMMON /DC/ 

ACC, 

WWB 

247 C 




248 

FLAG = 0 



249 

READ (IN, 10, 

END « 90, 

ERR - 90) SL, SA 

250 C 




251 C 

*** no error, 

generate 

matrices A and W 

252 C 




253 

CALL FUDGE (A 

, ACC, SL) 


254 

CALL FUDGE (W 

, WWB, SA) 


255 

RETURN 



256 90 

CONTINUE 



257 C 




258 C 

*** error condition 


259 C 




260 

FLAG = 1 



261 

RETURN 



262 10 

FORMAT (2011) 



263 

END 
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3 Type Offset P Class 


REAL-8 

REAL-8 

rt G I NT EGER *4 

INTEGERS 
TNTEGER*4 
INTEGER*4 
REAL*8 
7 REAL*8 


8 * 

0 /DC / 

4 * 

0 * 

414 
390 
12 * 

24 /DC / 


264 $PAGE 
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265 C 




266 C 




267 

SUBROUTINE FUDGE (A, ACC, SL) 



268 C 




269 C 




270 C 




271 C 

*** Sets appropriate components based on SL 



272 C 




273 C 




274 C 




275 

INTEGER SL(6) , T, K, J 



276 

REAL * 8 ACC(3) , A(3), X 



277 

DO 100 K - 1, 6, 2 


1 

278 

J = (K + 1) / 2 


1 

279 

X = 0.0 


1 

280 

T * SL(K) + SL(K+1 ) 


1 

281 

IF (T .EQ. 1) THEN 


1 

282 

X = ACC(J) 


1 

233 

IF (SL(K) .EQ. 0) X = -X 


1 

284 

END IF 


1 

285 

A( J) = X 


1 

286 100 

CONTINUE 



287 

RETURN 



288 

END 


Name 

Type 

Offset P Class 


A 

REAL-8 

0 * 


ACC 

REAL*8 

4 * 


J 

INTEGERS 

450 


K 

INTEGERS 

446 


SL 

INTEGERS 

8 * 


T 

INTEGER*4 

462 


X 

REAL*8 

454 



289 SPAGR 



0 M V »> 


/ s < 


I) Line" 1 

339 

340 C 

341 

342 


S ( 1 4 ) = MASS 

RETURN 

END 
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..ame Type 

REAL*8 
REAL*8 
CYCLE REAL*8 

"UL REAL* 8 

II REAL*8 

L REAL* 8 

' 3 REAL*8 

L REAL*8 

. iASS REAL*8 

MODE INTEGER *4 

INTEGER* 4 
MEGA REAL*8 

POS REAL*8 

REAL*8 
Q REAL*8 

^ REAL* 8 

>TEP INTEGER*4 

EL REAL*8 

REAL*8 


Offset P Class 


642 


570 


32 

/MC 

48 

/PC 

0 

/MC 

546 


498 


522 


24 

/MC 

40 

/MC 

714 


72 

/PC 

0 

/PC 


0 * 

466 

4 * 

44 /MC / 

24 /PC / 

8 * 


343 SPACE 


CHiSCADING PAGfi BLANK 


KOT FILMED 


<" 0 


Line# 

344 

345 

346 

347 

348 

349 

350 

351 

352 

353 

354 

355 

356 

357 

358 

359 

360 

361 

362 

363 
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C 

c 

SUBROUTINE PUT t S, A, M) 

C 

C— 

C 

C *** The procedure copies a vector A into a larger one S 

C starting at the N-th element of S 

C 

C 

c 

REAL * 8 S( 14) 

REAL * 8 A(M) 

C 

DO 100 K - 1, M 
N = N + 1 
S(N)= ACK) 

100 CONTINUE 
RETURN 
END 


ne Type 

REAL*8 

INTEGER*4 

INTEGERS 

INTEGER*4 

REAL*8 


Offset P Class 

8 * 

718 
12 * 

0 * 

4 * 


364 SPAGE 


O CJ > 


<<< 0 M V >>> 


D Line# 1 7 

365 C 

366 C 

367 SUBROUTINE DOTPRD (A, B, C, N) 

368 C 

369 C 

370 C 

371 C *** This procedure calculates a vector G from two other 

372 C vectors A and B such that 

373 C C(I) = A ( I ) * BCI) 

374 C for all i - 1 to N 

375 C 

376 C 

377 C 

378 REAL * 8 

379 DO 100 K 

1 380 C(K) 

1 381 100 CONTINUE 

382 RETURN 

383 END 

Name Type Offset P Class 

REAL*8 0 * 

REAL*8 4 * 

REAL*8 8 * 

K INTEGERS 726 

N INTEGER*4 12 * 


384 SPAGE 


A ( N ) , B(N), C(N) 
= 1, N 

* A ( K ) * B(K) 
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:« 0 M V 


»> 
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3 Line# 1 

385 C 

386 C 

387 

388 C 

389 C 

7 

SUBROUTINE 

Microsoft F0RTRAN77 V3.13 8/05/83 

DETQ (E, Q) 

390 C 



391 C 

*** calculates quaternions from the Euler angles 

392 C 

using 

expression given by Zack. 

393 C 



394 C 



395 C 



396 

REAL * 8 

E(3), Q(4) 

397 

REAL * 8 

Cl, SI, C2, S2, C3, S3, THETA 

398 C 



399 

THETA * E( 1 ) / 2.0 

400 

CALL SINCOS 

(THETA, SI, Cl) 

401 

THETA = E ( 2 ) / 2.0 

402 

CALL SINCOS 

(THETA, S2, C2) 

403 

THETA = E ( 3 ) / 2.0 

404 

CALL SINCOS 

(THETA, S3, C3) 

405 C 



406 

Q( 1 ) - SI * 

C3 * C2 + Cl * S3 * S2 

407 

Q(2) - SI * 

S3 * C2 + Cl * C3 * S2 

408 

0(3) - Cl * 

S3 * C2 - SI * C3 * S2 

409 

Q(4) - Cl * 

C3 * C2 - SI * S3 * S2 

410 C 



411 

RETURN 


412 

END 


ame Type 

Offset P 

Class 

REAL*8 

750 


i REAL*8 

766 


3 REAL*8 

782 


REAL*8 

0 * 


REAL* 8 

4 * 


1 REAL* 8 

742 


REAL*8 

758 


REAL*8 

774 


4 ETA REAL*8 

734 



413 SPAGE 
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414 C 

415 C 

416 SUBROUTINE SINCOS (THETA, S, C) 

417 C 

418 C 

419 C 

420 C 

421 C *** this procedure returns the sine and cosine of an 

422 C angle THETA. 

423 C 

424 C 

425 C 

426 REAL * 8 THETA, S, C, A 

427 C 

428 C = DC0S( THETA) 

429 S = DSIN(THETA) 

430 RETURN 

431 END 


Name 

Type 

Offset 

P 

Class 

A 

REAL *8 

***** 



C 

REAL *8 

8 

* 


DCOS 




INTRINSIC 

DSIN 




INTRINSIC 

S 

REAL *8 

4 

* 


THETA 

REAL -8 

0 

* 



432 SPAGE 


:« 0 M v 


>>> 


) Line# 

433 

434 

435 

436 

437 

438 

439 

440 

441 

442 

443 

444 

445 

446 

447 

448 

449 

450 

451 

452 

453 

454 

455 

456 

457 

458 

459 

460 

461 

462 

463 

464 

465 

466 

467 

468 

469 

470 

471 

472 

473 

474 

475 

476 

477 

478 

479 

480 
431 


1 

C 

C 


c 

G 

c 

c 

c 

G 

c 


c 


c 


c 

c 

c 

c 

c 

c 

c 


200 


c 

c 

c 

c 


c 

c 

c 

c 

c 
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SUBROUTINE MOTION (X, V, E, A, W, Q) 


♦** This procedure solves the equation of motion 


REAL * 8 
REAL * 8 
REAL * 8 
REAL * 8 
REAL * 8 
INTEGER 


POS(3), VEL(3) , EUL(3) , OMEGA 

X(3) , V(3) , E(3) , A(3) , W(3), Q(4) 

CIN(3,3) , C(3,3) , AA(3, 10) , B(3), QQ(4) 

WW(3) , PI, TWO 

111(3), MASS, CYCLE 

MODE, STEP 


COMMON /MC/ III, MASS, CYCLE, MODE, STEP 

COMMON /PC/ POS, VEL, EUL, OMEGA 


H = CYCLE / FLOAT (STEP) 
N « STEP 

PI * 355.0 / 113.0 
TWO* PI * 2.0 


*** Divide 1 major cycle into N equal subintervals and 
*** determine the OMV state for each interval 


DO 100 KK * 1, N 

*** Update orientation 

DO 200 J * 1, 3 
WW(J) = W(J) * H 
E(J) * E(J) + WW(J) 

IF (E(J) .GT. TWO) E(J) = E(J) - TWO 
CONTINUE 

*** Calculate quaternion for this rotation, and transform 
*** it to local vertical frame with respect to initial frame 

CALL DETQ( WW, QQ) 

CALL UPDQ (Q, QQ) 

*** from the direction cosine matrix, calculate the 
***' acceleration vector in LVF and store it in the 
*** acceleration matrix AA 

CALL DCSINV (Q, CIN) 


«< 0 M V 


D Line# 1 
1 482 

1 483 

1 484 100 

485 C 

486 C 

487 C 

488 C 

489 

490 C 

491 

492 


»> 


7 

CALL 

CALL 

CONTINUE 
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DMUL (CIN, A, B, 3) 

STORE (B, AA, KK) 


*** Solve the equation of motion using the Adam-Brashford 
*** method 

CALL SOLVE (X, V, AA, N, H, OMEGA) 

RETURN 

END 


Name 

Type 

Offset P Class 


A 

REA! *8 

12 * 


AA 

REAL*8 

990 


B 

REAL-8 

1230 



REAL*8 

918 


CIN 

REALMS 

846 


CYCLE 

KEAL*8 

32 /MC 

/ 

E 

REAL*8 

8 * 


EUL 

REAL*8 

48 /PC 

/ 

FLOAT 


INTRINSIC 

U 

REAL 

1254 


III 

REAL-8 

0 /MC 

/ 

J 

INTEGER *4 

1286 


KK 

TNTEGER-4 

1278 


MASS 

REAL-S 

24 /MC 

/ 

MODE 

INTEGER-4 

40 /MC 

/ 

V 

I NT EGER *4 

1258 


OMEGA 

REAL*8 

72 /PC 

/ 

PI 

REAL*8 

1262 


POS 

REAL-8 

0 /PC 

/ 

Q 

REAL*8 

20 * 


90 

REAL* 8 

814 


STEP 

IN'IT;GER*4 

44 /MC 

/ 

TWO 

REAL* 8 

1270 


V 

REAL*8 

4 * 


VEL 

REAL* 8 

24 /PC 

/ 

\\ 

REAL*S 

16 * 


WW 

REAL*8 

790 


X 

REAL*8 

0 * 



493 SPACE 


« o 


Line " 

494 

495 

496 

497 

498 

499 

500 

501 

502 

503 

504 

505 

506 

507 

508 

509 

510 

511 

512 

513 
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C 

C 

SUBROUTINE MATCH (A, B, C, P, Q, R, N) 

C 

C 

C 

C *** This procedure makes an exact duplicate B of a 

C vector A of N elements 

C 

c 

c 

REAL * 8 A(N), B(N), C(N), P(N), Q(N), R(N) 

DO 100 K = 1, 3 
P(K) = A(K) 

0(K) = B(K) 

R(K) = C(K) 

100 CONTINUE 
RETURN 
END 


ime Type 

REAL*8 

REAL*8 

REAL*8 

INTEGERS 

INTEGER*4 

REALMS 

REAL*8 

REAL # 8 


Offset P Class 

0 * 

4 * 

8 * 

1290 
24 * 

12 * 

16 * 

20 * 


514 SPACE 


<w 


0 M V 


D Line# 1 

515 C 

516 C 

517 

518 0 

519 C 


>>> 
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SUBROUTINE STORE (AAA, AA, K) 



u 

521 C 

522 C 

523 C 

524 C 

525 C 

526 C - 

This procedure takes an instantaneous acceleration 
AAA and stores it in the acceleration matrix AA which is 
by the numerical integration process 

vector 

needed 


527 C 






528 

REAL * 8 

AA(3, 10) 




529 

REAL - 8 

AAA(3) 




530 

DO 100 J = 

1. 3 



1 

531 

AA(J.K) 

= AAA(J) 



1 

532 100 

CONTINUE 





533 

RETURN 





534 

END 




Name 

Type 

Offset P 

Class 



AA 

REAL*8 

4 * 




AAA 

REAL-8 

0 * 




J 

INTEGERS 

1294 




K 

INTECER*4 

8 * 





535 SPACE 



0 


ine# 

536 

537 

538 

539 

540 

541 

542 

543 

544 

545 

546 

547 

548 

549 

550 

551 

552 

553 

554 

555 

556 

557 

558 

559 

560 

561 

562 

563 

564 

565 

566 

567 

568 

569 

570 

571 

572 

573 

574 

575 

576 

577 

578 

579 

580 

581 

582 

583 

584 
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C 

C 

SUBROUTINE SOLVE(X,V,A,N,H,W) 

C 

c 

c 

C This subroutine produces the numerical solution to the 

C system of equations of motion using a 3 step Adam-Brashford 

C method. 

C 

C 

c 

LOGICAL FLAG 

REAL*8 X(3), V(3), A(3,10), AA(3,13), U(6,13) 

REAL-8 WX2, WXW, WXWX3, HD12, F, W 

COMMON /BLOCK/ AA, U, WX2, WXW, WXWX3 , HD12 

DATA FLAG /.TRUE./ 

C 

C *** pack user supplied nonhomomgenous part of DE 

C *** into the higher part of AA 

C 

DO 10 I - 1,10 
DO 10 K = 1,3 

AA(K , 1+3) * A(K,I) 

10 CONTINUE 

C 

C *** if this is the first call to solve (FLAG * T), then 

C *** it is necessary to initialize some parameters 

C 

IF (FLAG) THEN 

CALL INNIT(X, V,W,II) 

FLAG = .FALSE. 

END IF 

C 

C *** use the Adams-Brashford 3-step method to advance the 

C *** solution H time units. Place the solution back into 

C *** X and V. 

C 

DO 100 I = 4.N+3 
DO 100 J =1,6 

U(J,I) = U(J.I-l) + 

+ HD12*(23*F(J,I-1)-16*F( J,I-2)+5*F(J,I-3)) 

100 CONTINUE 

X(l) = U( 1 ,N+3) 

V(l) = U(2,N+3) 

X(2) = U(3,N+3) 

V ( 2 ) = U(4,N+3) 

X(3) = U(5,N+3) 

V(3) = U(6,N+3) 
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585 C 





586 C 

*** reset U and AA for the 

next call to SOLVE 


587 C 





588 

DO 200 J 

= 1,6 


1 

589 

DO 200 I = 1,3 


') 

590 

U(J,I) * U( J ,N+I ) 


2 

591 

IF 

(J .LE. 3) AA ( I , J ) 

= AA ( I , N+ J ) 

2 

592 200 

CONTINUE 




593 

RETURN 




594 

END 



Name 

Type 

Offset 

P Class 


A 

REALMS 

8 

* 


AA 

REAL*8 

0 

/BLOCK / 


F 

REAL*8 


FUNCTION 


FUG 

L0GICAL*4 

1298 



H 

REAL 

16 

* 


IID12 

REAL*8 

960 

/BLOCK / 


r 

INTEGERS 

1302 



J 

INTEGER*4 

1314 



K 

INTEGER*4 

1306 



N 

INTEGER*4 

12 

* 


U 

REAL*8 

312 

/BLOCK / 


V 

REAL*8 

4 

* 


w 

REAL*8 

20 

* 


WX2 

REAL*8 

936 

/BLOCK / 


WXW 

REAL*8 

944 

/BLOCK / 


WXWX3 REAL*8 

952 

/BLOCK / 


X 

REAL*8 

0 

* 
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SUBROUTINE INNIT(X,V,W,H) 

OU 1 

602 C 

603 C 

604 C 

605 C 

606 C 

An7 r 

This procedure initializes all the necessary parameters 
before solving the system of ordinary differential equations. 
This procedure is invoked only once. 

OU / 


608 C 


609 

REAL * 8 X ( 3 ) , V(3) , AA(3,13), U(6,13), WX2, WXW, WXWX3 

610 

REAL * 8 CWT, SWT, T, W, HD12 

611 

COMMON /BLOCK/ AA, U, WX2, WXW, WXWX3 , HD12 

612 

WXW » W*W 

613 

WXWX3 = 3*WXW 

614 

WX2 - 2*W 

615 

HD12 * DBLE(H)/12.0 

616 C 


617 

DO 100 K - 1,3 

618 

U(2*K-1,3) * X(K) 

619 

U(2*K ,3) - V(K) 

620 

DO 100 J - 1,6 

621 

AA ( J , K ) = 0.0 

622 C 

CONTINUE 

623 100 

CONTINUE 

624 C 


625 

DO 300 I * 1,2 

626 

T = H*(I-3) 

627 

CWT = DCOS(W*T) 

628 

SWT - DSIN(W*T) 

629 

U(1,I) = X( 1) + V( 1)*(4*SWT-3*W*T)/W + 

630 + 

6*X(3)*(SWT-W*T) + 2*V(3)*(CWT-1.0)/W 

631 

U(2,I) = V(l)*(4*CWT-3.0) + 6*W*X(3)*(CWT-1.0) - 

632 + 

2*V(3)*SWT 

633 

u(3,i) » x(2)*cvrr + v( 2 )*swt/w 

634 

U(4,I) - -X(2)*W*SWT + V(2)*CWT 

635 

U(5, I) * 2*V(l)*(1.0-CVrr)/W + X(3)*(4.0-3*CWT) + 

636 + 

V(3)*SWT/W 

637 

U(6,I) = 2*V(1)*SWT + 3*X(3)*W*SWT + V(3)*CWT 

638 300 

CONTINUE 

639 

RETURN 

640 

END 

ame Type 

Offset P Class 

A REAL* 8 

0 /BLOCK / 



«< 0 M v 
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CWT 

REAL* 8 

1362 



)BLE 




INTRINSIC 

i)C0S 




INTRINSIC 

DSIN 

I 

REAL 

12 

* 

INTRINSIC 

ID 1 2 

REAL*8 

960 


/BLOCK / 

I 

INTF,GER*4 

1350 



1 

INTEGER*4 

1346 



( 

INTEGER*4 

1342 



SWT 

REAL*8 

1370 



T 

REAL*8 

1354 



i 

j 

REAL*8 

312 


/BLOCK / 

7 

REAL*8 

4 

* 


W 

REAL*8 

8 

* 


7X2 

REAL*8 

936 


/BLOCK / 

7XW 

REAL*8 

944 


/BLOCK / 

WXWX3 

REAL*8 

952 


/BLOCK / 

X 

REAL*8 

0 

* 
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641 SPACE 



<« 0 M v 


i) Line// 1 

642 C 

643 C 

644 

645 C 

646 C 

647 C~ 

648 C 

649 C 

650 

651 

652 C 

653 • 

654 10 

655 

656 

657 20 

658 

659 

660 30 

661 
662 

663 40 

664 

665 

666 50 

667 

668 

669 60 

670 

671 

672 


>'>> 
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FUNCTION F(J,I) 


REAL*8 AA(3, 13) ,U(6, 13) ,WX2,WXW,WXWX3,HD12,F 
COMMON /BLOCK/ AA f U , WX 2 , WXW , WXWX 3 , HD 1 2 

CO TO (10,20,30,40,50,60), J 
CONTINUE 
F = U ( 2 , 1 ) 

RETURN 

CONTINUE 

F = -WX2*U(6,I) + AA ( 1 , I ) 

RETURN 
CONTINUE 
F * U ( 4 , 1 ) 

RETURN 

CONTINUE 

F = -WXW*U(3,I) + AA(2,I) 

RETURN 
CONTINUE 
F = U(6,I) 

RETURN 
CONTINUE ' 

F = WX2*U(2,I) + WXWX3*U(5,I) + AA(3,I) 
RETURN 

END 


.'ame 

Type 

Offset 

P Class 


\A 

REALMS 

0 

/BLOCK 

/ 

ID 1 2 

REAI *8 

960 

/BLOCK 

/ 

i 

INTEGER*4 

4 

* 


J 

INTEGER*4 

0 

* 


f 

REAL* 8 

312 

/BLOCK 

/ 

;x2 

REAL-8 

936 

/BLOCK 

/ 

;xw 

REALMS 

944 

/BLOCK 

/ 

/XUX3 

REAL* 8 

952 

/BLOCK 

/ 


673 SPACE 



0 M V >>> Page 26 

07-14-84 

12:51:14 

in eil 1 7 Microsoft FORTRAN 77 V3.13 8/05/83 

674 C 


675 C 
bib 

677 C 

678 C. 

679 C 

680 C 

681 C 

682 C 

683 C 

684 C 

685 C 

686 C 

687 C 

688 C 

689 C 

690 

691 

692 

693 C 

694 

695 C 

696 

697 

698 

699 

700 

701 

702 C 

703 

704 

705 

706 

707 

708 

709 100 

710 C 

711 

712 10 

713 12 

714 15 

715 20 

716 30 

717 40 

718 1 

719 2 

720 50 

721 90 

722 


SUBROUTINE OUTPUT (A, W, X, V, E, Q, S, CMD, TIME) 


This is the output section of the system. Any further 
modification of the output requirements of this model must 
be done in this procedure. In particular, if no output to 
the CRT or printer is needed, it is recommended that C's 
be inserted into column 1 of all the WRITE statraents. The 
simulation clock is updated in this procedure. 


REAL * 8 A(3) , W(3), X(3), V(3), E(3), Q(4), S( 14) 

INTEGER CMD( 7) , EF, EEF, PRTFG 

INTEGER * 4 TIME, T 

COMMON /ME/ EF, EEF, PRTFG 


TIME = TIME + 1 

T = (TIME / 10) * 10 - TIME 
IF ( (T .NE. 0) .OR. (PRTFG .EQ. 0)) RETURN 
IF (PRTFG .EQ. 1) GO TO 100 
OPEN (4, FILE = 'LPT1 : ' ) 

WRITE (4, 15) TIME / 10 
WRITE (4, 10) A, W 
WRITE (4, 20) X, V 
WRITE (4, 30) E, W 
WRITE (4, 40) S 
WRITE (4, 50) CMD 
WRITE (4, 90) 

CLOSE (4) 

IF (PRTFG .NE. 2) CALL PLOT (CMD) 


RETURN 

FORMAT (* A, W 
FORMAT (' ', 7110) 
FORMAT (' TIME = ', 
FORMAT (' X, V =', 
FORMAT (' E, W 
FORMAT (’ S =’, 


FORMAT (' CMD =' , 
FORMAT ( 1H0) 

END 


3F10.6, 3X, 3F10.6) 

16, * Seconds') 
3F10.6, 3X, 3F10.6) 
3F10.6, 3X, 3F10.6/) 
3F10.6, 3X, 3F10.6/ 
3F10.3/ 

4F10.6, 3X.F10.3/) 
7110) 


«< 

0 M V »> 




D Line# 1 7 




Name 

Type 

Offset 

P 

Cla 

A 

REAL*8 

0 

* 


CMD 

INTEGER-4 

28 

* 


E 

REAL*8 

16 

* 


EEF 

INTEGER*4 

4 


/ME 

EF 

INTEGER *4 

0 


/ME 

PRTFG 

INTEGER-4 

8 


/ME 

Q 

REAL* 8 

20 

* 


s 

REAL*8 

24 

* 


r 

INTEGER*4 

1378 



TIME 

INTEGERS 

32 

* 


V 

REAL*8 

12 

* 


w 

REAL*8 

4 

* 


X 

REAL*8 

8 

* 
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Line# 1 

724 C 

725 C 

726 

727 C 

728 C 

729 C 

730 C 

731 C 

732 C 

733 C 

734 C 

735 C 

736 

737 C 

738 

739 

740 

741 

742 200 

743 

744 100 

745 

746 
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SUBROUTINE DMUL (A, B, C, N) 


This procedure performs a matrix multiplication of an NxN 
matrix A to an N-element column matrix B to yield an N-element 
column matrix C 


REAL * 8 A(N,N) , B(N), C(N), S 

DO 100 I = 1, N 

S = 0.0 

DO 200 J - 1. N 

S = S + A(I , J) * B(J) 
CONTINUE 
C(I) = S 
CONTINUE 
RETURN 
END 


le Type 

REAL* 8 

REAL*8 

REAL*8 

INTEGER*4 

INTEGER*4 

INTEGER*4 

REAL*8 


Offset P Class 

0 * 

4 * 

8 * 

1714 
1730 
12 * 

1722 


747 SPAGE 



«< 0 M V >>> 


D Lino# 1 

748 C 

749 C 

750 

751 C 

752 C 

753 G 

754 C 

755 C 

756 C 

757 C 

758 C 

759 C 

760 C 

761 C- 
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SUBROUTINE UPDQ (Q, QQ) 


This subroutine uses the previous quaternion and generates 
the present quaternions with restpect to the local vertical 
frame LVF. Quaternion algebra is used to deduce the needed 
computation before hand to simplify the algorithm 



762 C 

763 C 

764 

REAL * 8 Q(4), 


765 C 

766 

Ql * Q( 1 )*QQ(4) + 


767 

Q2 = Q(2)*QQ(4) + 


768 

Q3 = Q(3)*QQ(4) - 


769 

Q4 = Q(4)*QQ(4) - 


770 C 

771 

Q(l) = Ql 


772 

Q(2) = Q2 


773 

Q(3) = Q3 


774 

Q(4) * Q4 


775 

RETURN 


776 

END 

Name 

Type 

Offset P Class 

Q 

REAL*8 

0 * 

Ql 

REAL*8 

1738 

}2 

REAL*8 

1746 

^3 

REAL*8 

1754 

Q4 

REAL*8 

1762 

QQ 

REAL*8 

4 * 


QQ(4), Ql, Q2, Q3, Q4 

Q(4)*QQ(1) - Q(3)*QQ(2) + Q(2)*QQ(3) 
Q(3)*QQ(1) + Q(4)*QQ(2) - Q(1)*QQ(3) 
Q(2)*QQ(1) + Q(1)*QQ(2) + Q(4)*QQ(3) 
Q(1)*QQ(1) - Q(2)*QQ(2) - Q(3)*QQ(3) 


777 $PAGE 
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*.ine# 1 

778 C 

779 C 

780 

781 C 

782 C 

783 C 

784 C 

785 C 

786 C 

787 C 
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SUBROUTINE DCSINV (Q, C) 

This subroutine takes the attitude quaternion Q and returns 
the transpose of the direction cosine matrix 

l OO 



789 C 



790 C 



791 

REAL * 8 Q(4) , 0(3,3) 


792 

REAL * 8 Ql, Q2, Q3, Q4 


793 

REAL * 8 Qll, Q22, Q33, Q44 


794 

REAL * 8 Q12, Q13, Q23 


795 

REAL * 8 Q14, Q24, Q34 


796 C 



797 

Q1 = Q(l) 


798 

Q2 = Q(2) 


799 

Q3 = Q(3) 


800 

Q4 = Q(4) 


801 C 



802 

Qll = Q1 * Q1 


803 

Q22 = Q2 * Q2 


804 

Q33 = Q3 * Q3 


805 

Q44 = Q4 * Q4 


806 C 



807 

Q12 = 2.0 * Q1 * Q2 


808 

Q13 = 2.0 * Q1 * Q3 


809 

Q23 = 2.0 * Q2 * Q3 


810 

Q14 = 2.0 * Q1 * Q4 


811 

Q24 = 2.0 * Q2 * Q4 


812 

Q34 = 2.0 * Q3 * Q4 


813 C 



814 

C(l,l) - Qll - Q22 - Q33 + Q44 


815 

0(2,2) - -Qll + Q22 - Q33 + Q44 


816 

C(3,3) = -Qll - Q22 + Q33 + Q44 


817 C 



818 

C( 1 , 2) * Q12 - Q34 


819 

C(2, 1 ) * Q12 + Q34 


820 

C(l,3) = Q13 + Q24 


821 

0(3,1) = Q13 - Q24 


822 

0(2,3) = Q23 - Q14 


823 

0(3,2) = Q23 + Q14 


824 

RETURN 


825 

END 
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Name 

Type 

Of fset P Class 



C 

REAL-8 

4 * 



Q 

REALMS 

0 * 



Ql 

REAL*8 

1770 



Qll 

REALMS 

1802 



Q12 

REAL*8 

1834 



Q13 

REAL*8 

1842 



Q14 

REALMS 

1858 



Q2 

REAL*8 

1778 



022 

REAL*8 

1810 



Q23 

REAL*8 

1850 



Q24 

REAL*8 

1866 



Q3 

REAL*8 

1786 



Q33 

REAL :!! 8 

1818 



Q34 

REAL*8 

1874 



Q4 

REAL*8 

1794 



Q44 

REAL*8 

1826 



826 SPAGE 
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:e T y pe 

Size 

Class 

'OFRF. 


SUBROUTINE 

)CK 

968 

COMMON 

; 

48 

COMMON 

'SINV 


SUBROUTINE 

’Q 


SUBROUTINE 

.JL 


SUBROUTINE 

)TPRD 


SUBROUTINE 

REAL*8 


FUNCTION 

)GE 


SUBROUTINE 

iDCTL 


SUBROUTINE 

'ITPL 


SUBROUTINE 

nIIT 


SUBROUTINE 

\ IN 


PROGRAM 

vTCH 


SUBROUTINE 


48 

COMMON 


12 

COMMON 

)TI0N 


SUBROUTINE 

VMDL 


SUBROUTINE 

tput 


SUBROUTINE 

] 

80 

COMMON 

OT 


SUBROUTINE 

T 


SUBROUTINE 

.'ITGM 


SUBROUTINE 

IN COS 


SUBROUTINE 

LVE 


SUBROUTINE 

.ATE 


SUBROUTINE 

TORE 


SUBROUTINE 

'X 


SUBROUTINE 

’DQ 


SUBROUTINE 

XTOR 


SUBROUTINE 


ORIGINAL PAGE IS 
OE POOR QUALITY 


No Errors Detected 
826 Source Lines 
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i n e# 
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2 

3 

4 

5 

6 

7 

8 
9 

10 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 

43 

44 

45 

46 

47 

48 

49 
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SPACES I ZE : 56 

STITLE: ' « A D A M »' 

C 

C 

c 

C Program : ADAM 

C 

c 

C by 

C 

C Dr. W. Teoh 

C 

C 



c 

C This program uses the Adam Brashforth method to solve 

C the equation of motion (homogeneous case) numerically 

C and compares the solution with the analytical results 

C such that both outputs are printed. 

C 

C 



c 

c 

REAL*8 XE(3),VE(3),X(3),V(3),A(3,10),W 

REAL *8 X0(3) , V0(3) 

DATA A/ 30*0.0/ 

DATA N,H /10, 0.01/ 

C 

c 

c 

WRITE (*, 30) 

READ (*,32) W 
C 

C get initial conditions 

C 

CALL GETINT (XO, VO, 3) 

C 

DO 100 K - 1, 3 
X(K) » X0(K) 

V(K) = V0(K) 

100 CONTINUE 

C 

DO 10 I = 1,36000 
T = 0.1*1 
C 

C *** calculate the analytical solution 

C 

CALL EXACT(T,XE,VE,W,XO,VO) 



« ADA M >> 


D 

Line# 1 

7 Microsoft 

1 

50 C 


1 

51 C 

*** now get the numerical solution 

1 

52 C 

1 

53 

CALL S0LVE(X,V,A,N,H,W) 

1 

54 C 

1 

55 C 

*** output every 60 seconds 

1 

56 C 

1 

57 

JJ = (I / 600) * 600 

1 

58 

IF (JJ .EQ. I) THEN 

1 

59 

WRITE(*,20) T,XE, VE 

1 

60 

WRITE(*,20) T,X,V 

1 

61 

WRITE (*, 22) 

1 

62 

END IF 

1 

63 10 

CONTINUE 


64 C 

65 20 

FORMAT (F7.1, 6F12.6) 


66 30 

FORMAT (' ORBITAL RATE ’\) 


67 22 

FORMAT (1H ) 


68 32 

FORMAT (F15.8) 


69 

STOP 


70 

END 
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Name 

Type 

Offset 

A 

REAL*8 

146 

H 

REAL 

390 

I 

INTEGER-4 

406 

JJ 

INTEGER*4 

414 

.< 

INTEGER*4 

402 

N 

INTEGER-4 

386 

T 

REAL 

410 

V 

REAL*8 

98 

VO 

REAL*8 

122 

VE 

REAL* 8 

26 

W 

REAL*8 

394 

< 

REAL* 8 

50 

XO 

REAL*8 

74 

XE 

REAL*8 

2 


71 $PAGE 



Line// 1 

72 C 

73 C 

74 

75 

76 

77 


80 

81 

82 

83 

84 

85 

86 

87 

88 

89 

90 

91 

92 

93 

94 

95 

96 

97 

98 

99 
100 
101 
102 


C 

C 

C- 


78 C 

79 C 


C 

C 

C- 

C 

C 

C 
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SUBROUTINE EXACT(T,XE,VE,W,X,V) 


** This subroutine calculates the exact solution 
of the homogeneous ODEs 


REAL*8 XE(3),VE(3),CWT,SWT,W, WT, X(3), V(3) 

WT « W * T 

SWT = DSIN(WT) 

CWT = DCOS(WT) 

XE( 1 ) = X( 1) + (4 * SWT - 3*WT)*V(1)/W + 6*(SWT - WT)*X(3) 

1 + 2 * (CWT - 1) * V(3) /W 

XE(2) = CWT* X( 2) + SWT * V(2) / W 

XE(3) = 2 * (1 - CWT) * V(l) / W + (4 - 3 * CWT) * X(3) 

1 - SWT * V(3) / W 

VE( 1 ) = (4 * CWT -3) * V(l) + 6 * W * (CWT -1) * X(3) 

1 - 2 * SWT * V(3) 

VE(2) = CWT * V(2) - W * SWT * X(2) 

VE(3) = 2*SWT*V( 1) + 3*W*SWT*X(3) + CWT*V(3) 

RETURN 

END 


me 

Type 

Offset 

P 

Class 

T 

REAL*8 

488 



OS 




INTRINSIC 

:IN 




INTRINSIC 

T 

REAL-8 

480 




REAL 

0 

* 



REAL-8 

20 

* 


' 

REAL*8 

8 

* 



REAL-8 

12 

* 



REAL*8 

472 




REAL* 8 

16 

* 



REAL*8 

4 

* 
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105 C 

106 SUBROUTINE SOLVE(X,V,A,N,H,W) 

107 C 

108 C 

Page 4 
07-05-84 
21:33:40 
13 8/05/83 

110 C 



111 C 



112 C 

** This subroutine produces the numerical solution 


113 C 

to the system of equations of motion 


114 C 



115 C 



116 C 



117 C 



118 C 



119 C 



120 

LOGICAL FLAG 


121 

REAL*8 X(3), V(3) , A(3,10), AA(3,13), U(6,13) 


122 

REAL*8 WX2, WXW, WXWX3, HD12, F, W 


123 

COMMON /BLOCK/ AA, U, WX2, WXW, WXWX3 , HD12 


124 

DATA FUG /.TRUE./ 


125 C 



126 C 



127 C 

pack user supplied nonhomogeneous part of DE into 


128 C 

the higher part of AA 


129 C 



130 

DO 10 I - 1,10 


131 

DO 10 K = 1,3 


132 

AA(K,I+3) = A(K,I) 


133 10 

CONTINUE 


134 C 



135 C 

if this is the first call to solve (FLAG = T), then 

136 C 

initialize 


137 C 



138 

IF (FUG) THEN 


139 

CALL INNIT(X,V,W,H) 


140 

FUG = .FALSE. 


141 

END IF 


142 C 



143 C 

use the Adam-Brashford 3-step method to advance 


144 C 

the solution h time units. Place the solution 


145 C 

back into X and V. 


146 C 



147 

DO 100 I * 4,N+3 


148 

DO 100 J = 1,6 


149 

U(J,I) = U(J,I-1) + 


150 + 

HD12*(23*F(J, 1-1 )-16*F( J, 1-2 )+5*F( J, 1-3) ) 

151 100 

CONTINUE 


152 

X(l) = U(l,N+3) 



« ADAM 


» 
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ane# 1 

7 

153 

V(l) = U(2,N+3) 

154 

X ( 2 ) = U( 3, N+3) 

155 

V ( 2 ) = U(4,N+3) 

156 

X(3) = U(5,N+3) 

157 

V(3) = U(6,N+3) 

158 C 


159 C 

reset U and , 

160 C 


161 

DO 200 J = 1,6 

162 

DO 200 I * 1 

163 

U(J,I) = 1 

164 

IF (J .LE 

165 200 

CONTINUE 

166 C 

DO 300 I = 1,3 

167 C 

DO 300 K = 1 

168 C 

AA(K,I) = 

169 C300 

CONTINUE 

170 

RETURN 

171 

END 


21:33:40 
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for the next call to SOLVE 


U(J,N+I) 


AA(K,N+I) 


AA(I,N+J) 


ie Type Offset P Class 


REAL*8 

REAL*8 

REAL*8 

,G L0GICAL*4 
REAL 

.2 REAL*8 

INTEGER*4 
INTEGER-4 
INTEGERS 
INTEGER*4 
REAL*8 
REAL*8 
REAL* 8 
2 REAL*8 

V REAL*8 

■VX3 REAL* 8 
REAL* 8 


8 * 


0 

/BLOCK / 


FUNCTION 

496 


16 * 


960 

/BLOCK / 

500 


512 


504 


12 * 


312 

/BLOCK / 

4 * 


20 * 


936 

/BLOCK / 

944 

/BLOCK / 

952 

/BLOCK / 


0 * 


172 $PAGE 


5 
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D 

Line# 1 

173 C 

174 C 

175 

176 C 

177 C 

178 C 

7 

Microsoft FORTRAN77 V3.13 8/05/83 

SUBROUTINE INNIT(X, V,W,H) 


179 C 




180 C 




181 C 


This is the initialization routine which is called only once 


182 C 




183 C 




184 C — 

— 



185 C 




186 C 




187 


REAL * 8 X(3) , V(3) , AA(3,13), U(6,13), WX2, WXW, WXWX3 


188 


REAL * 8 CWT, SWT, T, W, HD12 


189 


COMMON /BLOCK/ AA, U, WX2, WXW, WXWX3, HD12 


190 


WXW = W*W 


191 


WXWX3 = 3*WXW 


192 


WX2 = 2*W 


193 


HD 12 = DBLE(H)/12.0 


194 C 




195 


DO 100 I = 1,3 

1 

196 


DO 100 J = 1,6 

2 

197 


AA(J,I) = 0.0 

2 

198 100 


CONTINUE 


199 


DO 200 K = 1,3 

1 

200 


U(2*K-1,3) = X(K) 

1 

201 


U(2*K ,3) = V(K) 

1 

202 200 


CONTINUE 


203 C 




204 


DO 300 I = 1,2 

1 

205 


T = H*(I-3) 

1 

206 


CWT * DC0S(W*T) 

1 

207 


SWT = DSIN(W*T) 

1 

208 


U(1,I) = X(l) + V(1)*(4*SWT-3*W*T)/W + 

1 

209 

+ 

6*X(3)*(SWT-W*T) + 2*V(3)*(CWT-1.0)/W 

1 

210 


U(2,I) = V(l)*(4*CWT-3.0) + 6*W*X(3)*(CWT-1 .0) - 

l 

211 

+ 

2*V(3)*SWT 

1 

212 


U(3,I) = X(2)*CWT + V(2)*SOT/W 

1 

213 


U(4,I) = -X(2)*W*SWT + V(2)*CWT 

1 

214 


U(5,I) = 2*V( 1 )*( 1 ,0-CWT)/W + X(3)*(4.0-3*CWT) + 

1 

215 

+ 

V(3)*SWT/W 

1 

216 


U(6,I) = 2*V( 1)*SWT + 3*X(3)*W*SWT + V(3)*CWT 

1 

217 300 


CONTINUE 


218 


RETURN 


219 


END 



n o 
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uineti 1 7 
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: e Type Offset P Class 


A 

REAL*8 

0 


/BLOCK / 


REAL*8 

560 



i E 




INTRINSIC 

COS 




INTRINSIC 

STN 

REAL 

12 

* 

INTRINSIC 

u 1 2 

REAL*8 

960 


/BLOCK / 


INTEGER*4 

540 




INTEGER*4 

544 




INTEGER*4 

548 



,vT 

REAL*8 

568 




REAL* 8 

552 




REAL* 8 

312 


/BLOCK / 


REAL*8 

4 

* 



REAL*8 

8 

* 


l 

REAL*8 

936 


/BLOCK / 

J 

REAL* 8 

944 


/BLOCK / 

iWX3 

REAL* 8 

952 


/BLOCK / 


REAL*8 

0 

* 



220 $PAGE 


ORIGINAL page is 
BE POOR QUALITY 


ro Lo 


« 


ADAM >> 


-a 1 

21 C 

22 C 

23 

24 C 

25 C 

26 C 

27 C 

28 C 

29 C 

30 C 


7 


FUNCTION F(J,I) 


User supplied function 
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31 C 

32 C- 

33 C 

34 C 

35 

36 

37 C 

38 

39 10 

40 

41 

42 20 

43 

44 

:45 30 

:46 

147 

;48 40 
149 

250 

251 50 

252 

253 

254 60 

255 

256 

257 


REAL*8 AA ( 3 , 1 3 ) , U( 6 , 13 ) , WX2 , WXW , WXWX3 , HD1 2 , F 
COMMON /BLOCK/ AA,U,WX2,WXW,WXWX3,HD12 

GO TO (10,20,30,40,50,60), J 
CONTINUE 

F = U(2, I) 

RETURN 

CONTINUE 

F = -WX2*U(6,I) + AA( 1 , I) 

RETURN 

CONTINUE 

F = U(4, I) 

RETURN 

CONTINUE 

F = -WXW*U(3,I) + AA(2,I) 

RETURN 

CONTINUE 

F » U(6,I) 

RETURN 

CONTINUE 

F = WX2*U(2,I) + WXWX3*U(5,I) + AA(3,I) 
RETURN 

END 


Type 

REAL*8 
REAL* 8 
INTEGER *4 


INTEGER*4 

0 * 



REAL*8 

312 

/BLOCK 

/ 

REAL*8 

936 

/BLOCK 

/ 

REAL*8 

944 

/BLOCK 

/ 

23 REAL*8 

952 

/BLOCK 

/ 

258 SPACE 





Offset P Class 

0 /BLOCK / 

960 /BLOCK / 



« 
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1 SPAGF.SIZE: 56 

2 STITLE: '«< S V X »>’ 

3 C 

4 C 

5 C STATE VECTOR TRANSFORMATION MODULE (SVX) 

6 C 

7 C by 

8 C 

9 C 

10 C Dr. W. Teoh 

11 C 

12 C UAH 1984 

13 C 

15 C 

16 C 

17 SUBROUTINE SVX (S, CMDRAW, MODE) 

18 C 

19 C 

20 C 

21 C 

22 C This is the state vector transformation module which accepts a 

23 C 14 element state vector S of the OMV as input and generates a 

24 C 6-element command string CMDRAW as output. The argument MODE 

25 C conveys the following meaning : 

26 C 


27 C 

MODE 


Meaning 

28 C 

0 


rate control 

29 C 

1 


position control 

30 C 

anything else 

defaults to 1 

31 C 




32 C 

Summary of the 

state 

vector components are as follows : 

33 C 




34 C 

Component 


Meaning 

35 C 




36 C 

1 

X 

position of target vehicle from the 

37 C 

2 

Y 

chase vehicle in LVF 

38 C 

3 

Z 


39 C 

4 

vx 

relative velocity of the two vehicles 

40 C 

5 

VY 

in LVF 

41 C 

6 

VZ 


42 C 

7 

LX 

angular momentum vector in LVF 

43 C 

8 

LY 


44 C 

9 

LZ 


45 C 

10 

Ql 

attitude quaternions in body frame 

46 C 

11 

Q2 


47 C 

12 

Q3 


48 C 

13 

Q4 


49 C 

14 

M 

instantaneous mass in kg. 
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50 C 


51 

52 

53 

54 

55 

56 

57 

58 

59 

60 
61 
62 

63 

64 

65 

66 

67 

68 

69 

70 

71 

72 

73 

74 

75 

76 

77 

78 

79 

80 
81 
82 

83 

84 

85 

86 

87 

88 

89 

90 

91 

92 

93 

94 

95 

96 

97 

98 


C 


c 

c 

Summary of command string 

components: 

C 

component 

meaning 

coord system 

C 

1 

YAW 

body frame 

C 

2 

X 

floor coordinate 

C 

3 

Y 

floor coordinate 

C 

4 

Z 

floor coordinate 

C 

5 

PITCH 

body frame 

c 

6 

ROLL 

body frame 

c 

7 

MODE 

integer 


C 

C This module maintains a local counter to process initial 
C conditions at the start of the simulation. 

C 

C 

c 

c 

REAL * 8 S( 14) 

REAL * 8 X(3), V(3), L(3), Q(4) 

REAL * 8 X0(3) , XM(3), E(3), XH0LD(3) 

REAL * 8 IINV(3), LB(3), W(4) 

REAL * 8 RPY(3) , QD0T(4), QW(4,4), A(3,3) 

REAL * 8 LL, UL, UA, CC, AA, HH, QQ, TX, TY, Z 

REAL * 8 ROLL, PITCH, YAW, ROLDOT, PITDOT, YAWDOT 
REAL * 8 Ql, Q2, SY, CY, VX, VY, VZ 
C 

INTEGER CMDRAW(7) , COUNT, MODE 

C 

C *** load-time initialization 

C 

DATA COUNT /0/ 

C 

C *** decompose state vector and process it 

C 

CALL DECOMP (S, X, V, L, Q) 

IF (COUNT .NE. 0) GOTO 300 
C 

C *** initialization before start 

C 

CALL ZERO (XO, 3) 

C 

C *** read parameters 

C 

OPEN (1, FILE = 'SVXINT.DAT' , STATUS = ’OLD’) 

READ (1, 20) CC, LL, AA, HH 
READ (1, 20) IINV 



«< s v x >» 


D Line# 1 
99 C 

100 c 

101 c 

102 
1 103 

1 104 50 

105 

106 C 

107 C 

108 c 

109 

110 
111 

112 C 

113 C 
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*** calculate inverse of moment of inertia tensor 

DO 50 K s 1, 3 

IINV(K) = 1.0 / IINV(K) 

CONTINUE 
CLOSE (1) 

*** set conversion factors 

UL = 10000.0 
UA = UL 

COUNT = COUNT + 1 

*** set transformation matrix elements to floor coord. 


114 C 


115 

EC 1) = CC + LL - X0(1) 

116 

E(2) = AA - X0(2) 

117 

E( 3) - HH - X0(3) 

118 C 


119 C 

*** initialize to home orientation 

120 C 


121 

CALL ZERO (RPY, 3) 

122 

COUNT = COUNT + 1 

123 C 


124 300 

IF (MODE .N'E. 1) GO TO 400 

125 C 


126 C 

*** position commands 

127 C 


128 C 

*** update orientation and position 

129 C 


130 

CALL QTRPY (Q, ROLL, PITCH, YAW) 

131 

CALL UPDPOS (XM, X, XHOLD, E, 3) 

132 C 


133 C 

*** set orientation part of the command 

134 C 


135 

CMDRAW( 7 ) = 1 

136 

CMDRAW(6) =» JFIX ( ROLL * UA) 

137 

CMDRAW(5) = JFIX (PITCH * UA) 

138 

CMDRAW(l) = JFIX (YAW * UA) 

139 C 


140 C 

*** transform to T0M_B position in floor 

141 C 


142 

QO = CC + LL * DC0S( PITCH) 

143 C 


144 C 

*** X-component 

145 C 


146 

TX = XM ( 1 ) - QQ * DCOS(YAW) 

147 

CMDRAW(2) * JFIX (TX * UL) 



S V X »> 


< 


_ino!> 1 

148 C 

149 0 

150 C 

151 

152 

153 C 

154 C 

155 C 

156 

157 

158 C 

159 C 

160 C 

161 C 

162 

163 C 

164 400 

165 C 

166 C 

167 C 

168 

169 C 

170 C 

171 C 

172 C 

173 

174 

175 C 

176 C 

177 C 

178 

179 

180 

181 C 

182 C 

183 C 

184 

185 

186 

187 

188 C 

189 C 

190 C 

191 

192 

193 

194 

195 C 

196 C 
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*** Y-component 

TY = XM(2) - OQ * DSIN(YAW) 
CMDRAW(3) = JFIX (TY * UL) 

*** 2-component 

Z = XM(3) - LL * DSIN( PITCH) 
CMDRAW(4) = JFIX (Z * UL) 


*** This is a good place to call the I/O driver to 
*** transmit to T0M_B, but we won't for now 

RETURN 

IF (MODE .ME. 0) GO TO 900 
*** rate control 

CALL QTRPY (Q, ROLL, PITCH, YAW) 

*** form direction cosine matrix and calculate angular 
*** momentum in body frame 

CALL DIRCOS (A, Q) 

CALL MMUL (A, L, LB, 3) 

*** compute body rate 

ROLDOT = IINV(l) * LB(1) 

PITDOT » IINV(2) * LB(2) 

YAWDOT = IINV(3) * LB(3) 

*** construct orientation part of command string 
CMDRAW(7) = 0 

CMDRAW(6) = JFIX (ROLDOT * UA) 

CMDRAW(5) = JFIX (PITDOT * UA) 

CMDRAW(l) = JFIX (YAWDOT * UA) 

*** compute velocity of T0M_B in floor coordinates 

Q1 = LL * DSIN( PITCH) * PITDOT 

Q2 = (CC + LL * DC0S( PITCH) ) * YAWDOT 

SY = DSIN(YAW) 

CY = DCOS(YAW) 

*** X-component of velocity in floor coordinate 



<<< S V X 


D Line# 1 

197 C 

198 

199 

200 C 

201 C 

202 C 

203 

204 

205 C 

206 C 

207 C 

208 

209 

210 

211 C 

212 900 

213 C 

214 C 

215 C 

216 C 

217 

218 

219 C 

220 10 
221 20 
222 
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VX * V(l) + Q1 * CY + Q2 * SY 
CMDRAW ( 2 ) = JFIX (VX * UL) 

*** Y-component of velocity in floor coordinate 

VY = V(2) + Q1 * SY - Q2 * CY 
CMDRAW ( 3 ) « JFIX (VY * UL) 

*** Z-component 

VZ = V ( 3 ) - LL * DCOS ( PITCH ) * PITDOT 
CMDRAW(4) * JFIX (VZ * UL) 

RETURN 

CONTINUE 

*** We have an un-recognizable code, default to 1 for 
*** position control 

MODE = 1 
GO TO 300 

FORMAT (4F10.2) 

FORMAT ( F15.8) 

END 


Name 

Type 

\ 

REAL*8 

A A 

REAL*8 

ZC 

REAL*8 

JMDRAW 

INTEGER*4 

30UNT 

INTEGER*4 

:y 

REAL*8 

)C0S 

JSIN 

REAL*8 

1H 

REAL*8 

INV 

REAL* 8 

/ 

\ 

INTEGER*4 

, 

REAL*8 

,B 

REAL*8 


REAL*8 

10DE 

INTEGER*4 

’ITCH 

REAL*8 

TTDOT REAL*8 

) 

REAL*8 

U 

REAL*8 


Offset P Class 

466 

558 

542 

4 # 

538 

698 

INTRINSIC 
INTRINSIC 
418 
566 
442 
574 
370 
394 
550 
8 * 

602 

658 

154 

674 
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: REAL* 8 

682 

~T REAL*8 

210 

REAL*8 

618 

REAL*8 

242 

>1 DOT REAL*8 

650 

L REAL*8 

594 

, REAL*8 

186 

REAL*8 

0 

REAL*8 

690 

REALMS 

626 

REALMS 

634 

REAL*8 

586 

REAL*8 

578 

REAL*8 

98 

REAL*8 

706 

REAL*8 

714 

REAL*8 

722 

REAL*8 

122 

REAL*8 

2 

REAL*8 

26 

OLD REAL*8 

74 

REAL*8 

50 

V REAL*8 

610 

WDOT REAL*8 

666 

REAL*8 

642 

223 SPAGE 
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D Line# 1 7 

224 C 

225 C 

226 SUBROUTINE DECOMP (S, X, V, L, Q) 

227 C 

228 C 

229 C 

230 C This procedure decomposes the State vector S into its components 

231 C which are also vectors. They have the following meaning : 

232 C 

233 C Vector Dimension Meaning 

234 C X 3 Position vector in LVF 

235 C V 3 Velocity vector in LVF 

236 C L 3 Angular momentum in LVF 

237 C Q 4 Unit quaternion in body frame 

238 C 

239 C 

240 C 

241 REAL * 8 S(14), X(3), V(3), L(3), Q(4) 

242 C 

243 CALL LD (S, X, 1, 3) 

244 CALL LD (S, V, 4, 3) 

245 CALL LD (S, L, 7, 3) 

246 CALL LD (S, Q, 10, 4) 

247 C 

248 RETURN 

249 END 
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Name Type 

L REAL*8 
0 REAL*8 
S REAL*8 
V REAL*8 
X REAL*8 


Offset P Class 

12 * 

16 * 

0 * 

8 * 

4 * 


250 SPAGE 


< s 


nine# 

251 

252 

253 

254 

255 

256 

257 

258 

259 

260 
261 
262 

263 

264 

265 

266 
267 
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C 

C 

SUBROUTINE LD (A, B, M, N) 

C 

C 

c 

C This procedure copies N elements of vector A to vector B, 

C starting at the M-th element 

C 

C 

c 

REAL * 8 A( 14) , B(N) 

DO 100 K * 1, N 

B(K) - A(M + K - 1) 

100 CONTINUE 

RETURN 
END 


ne Type 


Offset P Class 


REAL*8 0 * 

REAL*8 4 * 

INTEGERS 750 

INTEGERS 8 * 

INTEGERS 12 * 


268 SPAGE 


<« S V x 


>>> 


D Line# 

269 

270 

271 

272 

273 

274 

275 

276 

277 

278 


1 

C 

C 

C 

C- 

c 

c 

c 

c 

c 
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SUBROUTINE MMUL (A, B, C, N) 


This procedure performs a matrix multiplication of an NxN 
matrix A to an N-eleraent column matrix B to yield an N- 
element column matrix C 



279 C 



280 C 



281 

REAL * 8 A(N,N) , 


282 C 



283 

DO 100 I a 1, N 

1 

284 

S = 0.0 

1 

285 

DO 200 J = 1, N 

2 

286 

S = S + A(I , J) 

2 

287 200 

CONTINUE 

1 

288 

C(I) = S 

1 

289 100 

CONTINUE 


290 

RETURN 


291 

END 

Name 

Type 

Offset P Class 

A 

REAL*8 

0 * 

B 

REAL*8 

4 * 

C 

REAL*8 

8 * 

I 

INTEGER*4 

758 

J 

INTEGERS 

774 

N 

INTEGER*4 

12 * 

S 

REAL*8 

766 


292 SPAGE 



N S 


ine# 

293 

294 

295 

296 

297 

298 

299 

300 

301 

302 

303 

304 

305 

306 

307 

308 

309 
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C 

c 

SUBROUTINE ZERO (A, N) 

C 

C 

C 

C This procedure initializes an N-element array A to zero at 
C run time 

C 

C 

REAL * 8 A(N) 

DO 100 K - 1, N 
A(K) - 0.0 
100 CONTINUE 

RETURN 
END 


ne Type 

REAL*8 

INTEGER*4 

INTEGERS 


Offset P Class 

0 * 

782 

4 * 


310 SPAGE 


<« 


D Line# 

311 

312 

313 

314 

315 

316 

317 

318 

319 

320 

321 

322 

323 

324 

325 

326 

327 

328 

329 

330 
1 331 

332 

333 


V X 


1 

C 

C 

C 

C- 

C 

C 

C 

C 

C 

C 

C 

C- 

C 

c 

c 


100 


»> 
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SUBROUTINE UPDP0S (XM, X, XH0LD, E, N) 


This procedure updates the position of the 0MV in local vertical 
frame (XH0LD) . 

The new position of the module in floor coordinates is then com- 
puted (XM) 


REAL * 8 XM(N) , X(N), XHOLD(N) , E(N) 

DO 100 K = 1, N 
XHOLD(K) = X(K) 

XM(K) = XHOLD(K) + E(K) 

CONTINUE 

RETURN 

END 


Name Type 

REAL*8 

K INTEGER *4 

INTEGER*4 
REAL*8 
XIIOLD REAL*8 
”M REAL- 8 


Offset P Class 

12 * 

790 
16 * 

4 # 

8 * 

0 * 


334 $PAGE 



S V X 

ine# 1 

335 C 

336 C 

337 

338 C 

»> 

7 
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33 c ) C 



340 C 



341 C 

This procedure properly rounds a 

real number R to the nearest 

342 C 

integer . 


343 C 



344 C 



345 C 



346 

REAL * 8 RR 


347 

REAL R 


348 

R = RR 


349 

IF (R .GE. 0) THEN 


350 

JFIX = IFIX (R + 0.5) 


351 

ELSE 


352 

JFIX = IFIX (R - 0.5) 


353 

END IF 


354 

RETURN 


355 

END 


me Type 

Offset P Class 


IX 

INTRINSIC 


REAL 

798 


REAL*8 

0 * 



356 SPAGE 


«< S V X 


D Line# 1 

357 C 

358 C 

359 

360 C 

361 C~ 

362 C 

363 C 

364 C 

365 C 

366 C 

367 C 

368 C 

369 C~ 

370 C 

371 

372 C 

373 

1 374 

2 375 

2 376 

2 377 

2 378 

2 379 

2 380 

2 381 110 

1 382 

1 383 100 

384 

385 C 

386 
1 387 

1 388 

2 389 

2 390 200 

391 

392 
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SUBROUTINE SETQ (QW, Q) 


This procedure constructs a 4x4 transformation matrix QW from 
the attitude quaternions Q 

For reference, please see "Software Specifications For Docking 
Simulation Of The 0MV" by J. Micheals, January, 1984, 


REAL * 8 QW(4,4) , Q(4) 

DO 100 I * 1, 3 

DO 110 J » 1+1, 4 
KK = I + J 
K = KK - (KK/4) * 4 
IF (K ,EQ. 0) K * 2 
ISGNN * 1 

IF ((J ,EQ. 1+1) .AND. ( J.NE. 4)) ISGNN « -1 
QW(I.J) = ISGNN * Q(K) 

CONTINUE 
QW(I.I) = Q(4) 

CONTINUE 
QW(4,4) - Q(4) 

DO 200 I = 2, 4 
KK = I - 1 
DO 200 J = 1, KK 

QW(I.J) - -QW(J.I) 

CONTINUE 

RETURN 

END 


Name 

Type 

r 

INTEGER*4 

ISGNN 

INTEGER*4 

] 

INTEGER*4 


INTEGERS 

:k 

INTEGER*4 

7 

REAL*8 

M 

REAL*8 


Offset P Class 

802 
818 
806 
814 
810 
4 * 

0 * 


393 SPAGE 



S V X 


»> 


ine# 

394 

395 

396 

397 

398 

399 

400 

401 

402 

403 

404 

405 

406 

407 

408 

409 

410 

411 

412 

413 

414 

415 

416 

417 

418 

419 

420 

421 

422 

423 

424 

425 

426 

427 

428 

429 

430 

431 

432 

433 

434 

435 

436 

437 


1 

C 

C 

C 

c- 

c 

c 

c 

c 

G 

c 

c 

c 

c 

c 

c 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


100 
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SUBROUTINE DIRCOS (A, Q) 


This procedure takes the quaternion vector and generates 
a 3 X 3 direction cosine matrix A 


REAL * 8 Q(4) , A(3,3), QKS, QRS, SI 

DO 100 K - 1, 3 

*** initialize diagonal elements 

A(K,K) = Q(4) ** 2 
DO 100 J = 1, 3 

*** fix up the diagonal elements 
A(K,K) = A(K,K) + DLTKRK(K.J) * Q(J) ** 2 


*** now do the off-diagonal elements 

IF ( J .GT. K ) THEN 

*** calculate index I <> J & K 

I - 6 / (J * K) 

*** calculate the proper sign 

SI = QSIGN (K, J) 

QKJ = Q(K) * Q( J) 

QRS = Q(I) * Q(4) * SI 
A(K, J) = 2.0 * (QKJ + QRS) 

A(J,K) = 2.0 * (QKJ - QRS) 

END IF 
CONTINUE 
RETURN 
END 


ame Type 


Offset P Class 

0 * 

838 


REAL*8 

INTEGER*4 


«< 

S V X »> 


D Line# 1 7 


J 

INTEGER*4 

830 

K 

INTEGER *4 

826 

Q 

REAL*8 

4 

QKJ 

REAL 

854 

QKS 

REAL*8 

***** 

QRS 

REAL*8 

858 

SI 

REAL*8 

842 
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438 $PAGE 



< S V X »> 


_ine# 1 7 

439 C 

440 C 

441 REAL FUNCTION DLTKRK (K,J) 

442 C 

443 C 

444 C 

445 C 

446 C 

447 REAL S 

448 INTEGER K, J 

449 S = 1.0 

450 IF (K .NE. J) S - -1.0 

451 DLTKRK = S 

452 RETURN 

453 END 

;i ie Type Offset P Class 

INTEGER*4 4 * 

INTEGER-4 0 * 

REAL 866 


454 $PAGE 


rosoft 
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D Line# 1 

455 C 

456 C 

457 

458 C 

459 C 

460 C 

461 C 

462 C 

463 

464 

465 

466 

467 

468 
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REAL FUNCTION QSIGN(K,J) 


S = 1.0 
L - J + K 

IF (MOD(L,2) .EQ. 0) S = -1.0 

QSIGN * S 

RETURN 

END 


Name 

Type 

Offset 

P 

Class 

J 

INTEGER*4 

4 

* 


K 

INTECER*4 

0 

* 


L 

INTEGER *4 

874 



MOD 




INTRINSIC 

S 

REAL 

870 




469 $PAGE 


m 


s v x >» 
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470 C 

471 C 

472 

473 C 

474 C 

475 C 

476 C 

477 C 

478 C 

479 

480 C 

481 

482 C 

483 C 

484 C 

485 

486 

487 

488 

489 

490 

491 

492 

493 

494 C 

495 C 

496 C 

497 

498 C 

499 C 

500 C 

501 

502 

503 

504 

505 
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SUBROUTINE QTRPY (Q, R, P, Y) 


This subroutine calculates a reasonable set of roll, 
pitch and yaw from the quaternion Q 

REAL * 8 Q(4) , R, P, Y, M, THETA, CA, CB, CG 

M = DSQRT (Q(l)**2 + Q(2)**2 + Q(3)**2) 

calculate direction cosines CA, CB, CG 

IF (DABS(M) .LE. 1.0D-20) THEN 
CA = 0.0 
CB = 0.0 
CG = 0.0 
ELSE 

CA = Q( 1) / M 
CB = Q(2) / M 
CG = Q(3) / M 
END IF 

calculate angle of rotation about Euler axis 

THETA = 2.0 * DAC0S(Q(4) ) 

now determine the roll, pitch and yaw 

R = CA * THETA 
P » CB * THETA 
Y = CG * THETA 
RETURN 
END 


ame Type 


Offset P Class 


\BS 

\C0S 

QRT 


REAL*8 

REAL*8 

REAL*8 


REAL*8 

REAL-8 

REAL*8 

REAL*8 


886 

894 

902 

INTRINSIC 

INTRINSIC 

INTRINSIC 


878 
- 8 * 
0 * 
4 * 


«< S V x »> 


D Line# 1 7 

THETA REAL*8 
Y REAL*8 


910 
12 * 
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~~ie 

Type 

tiCOMP 


IRCOS 


:krk 

REAL 

. IX 

INTEGER*4 

D 


JL 


IGN 

REAL 

TRPY 


"TQ 


( 


n)P0S 



SRO 


Size Class 

SUBROUTINE 

SUBROUTINE 

FUNCTION 

FUNCTION 

SUBROUTINE 

SUBROUTINE 

FUNCTION 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 

SUBROUTINE 


i’ass One No Errors Detected 
506 Source Lines 
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Mobility-base Control Logic (TOM_C) Source Listing 
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ORIGINAL PA£B 1 % 

OF POOR QUALITY 


TOM-B EXECUTIVE 


Version 11.2 

This is the main program for the OMV on-board processing 
logic. 

The following steps are carried out: 

1. Performs a system initialization 

2. Set up an infinite loop to Drocess each major cycle 
until both CMDRAW(l) & CMDRAW<2) are <* -99. 

During each major cycle, a subroutine PMAJOR 
performs all the necessary functions. It then waits 
for the next cycle. The start of the next cycle is 
indicated when FLAG is cleared. 

Each major cycle has a period of 0.1 sec; this value is in- 
put from disk during system initialization. Since 
cycle execution is tracked by using this variable, 
the period may be altered by changing its 
value on disk. 

Absolute commands will be used throughout. 

It is assumed that a routine SETUP sets an interrupt schecule 
and performs all the necessary services. 


INTEGER * 4 FLAG, CMDMOD, CMDRAW<9), CMDRETO) 

INTEGER a 4 CYCLE 

COMMON /CMMD/* CMDRET, CMDRAW, CMDVAL(9), CMDMOD , FLAG 
COMMON /CYCL/ CYCLE, JSTF1 

*** system initialization aa* 

CALL INITOM 

CALL G0H0ME 

aaa MONITOR CYCLE PROCESS aaa 

WHILE ( (CMDRAW(3) .GT.-99) .AND. ( CMDRAW( 2 ) . GT. -99 ) ) DO 

IFl ( CMDRAW ( 3 ) . LT. -99 ) . AND. ( CMDRAW ( 2 ) .LT.-99) )G0T0 900 

*** PROCESS A MAJOR CYCLE aaa 

CALL PMAJOR 

a** WAIT UNTIL NEXT CYCLE & CONTINUE *** 

CALL WAIT 
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ORIGINAL PAGE IS 
OF POOR QUALITY 


GO TO 100 
C END WHILE 
900 CONTINUE 

*** perform house cleaning before quitting *** 

CALL GOHOME 
STOP 
END 


SUBROUTINE INITOM 


This procedure performs a system initialization. 

1. A disk data file called INITOM is accessed for the 
pertinent information. 

2. power to disk drive may then be disconnected 

3. press <CR> to continue. 

4. INIT calls SETUP to establish an interrupt schedule. 


INTEGER 

* 4 

FLAG, CMDMOD, 

CMDRAW < 9 ) , 

CMDRET ( 9 ) 

INTEGER 


DOF 



INTEGER 

* 2 

FRTBLX (20,2) , 

FRTBLY( 20 , 2 ) 

, JETBUFf 40 ) 

REAL 


LX, LY, 

MASS. 

MAJOR , JZZ 

REAL 


MTRVRD< 6 ) , 

MTRVCL ( 6 ) , 

MTRVOF { 6 ) 

REAL 


NAWAL ( 3 ) , 

NAVCAL ( 3 ) , 

NAVOFF ( 6 ) 

REAL 


MTRPRD(6) , 

MTRPCL ( 6 ) , 

MTRPOF ( 6 ) 

COMMON 

/CMMD/ 

CMDRET. CMDRAW, 

CMDVAL ( 9 ) , 

CMDMOD, FLAG 

COMMON 

/DACO/ 

DACRDG ( 6 ) , 

DACCAL ( 6 ) , 

DACCOF( 6 ) 

COMMON 

/DYNA/ 

THRUST, 

ACC ( 2 ) , 

LX. LY. DOF 

COMMON 

/JETS/ 

NTHRX, NTHRY, FRTBLX, FRTBLY 

, JETBUF.SCLX.l 

COMMON 

/MOTR/ 

MTRPRD, 

MTRPCL, 

MTRPOF 

COMMON 

/MOTV/ 

MTRVRD, 

MTRVCL, 

MTRVOF 

COMMON 

/ NA VG / 

NAWAL, 

NAVCAL. 

NAVOFF 

COMMON 

/PHYS/ 

MASS. 

MAJOR, 

JZZ, PIRAD 

COMMON 

/POSN/ 

POSTN( 9 ) , 

OPOSTN ( 9 ) 


COMMON 

/RATE/ 

VLCTY ( 9 ) , 

OLDVEL ( 9 ) 


COMMON 

/SNSR/ 

SNRR ( 3 ) , 

SNRC < 3 ) , 

SNRB( 3 ) 

COMMON 

/PRCN/ 

EPSL, EPSA, UL, 

UA 


COMMON 

/DELV/ 

DV(3) 



Whew 





WRITE ( * ,39 ) 




FORMAT 

( ' in 

INITOM' ) 



Implementation 

notes : 



PHY. QTY 

STANDARD 


MKS 

MASS 


77.64 SLUG <2500 

LB) 1132 

.77 KG 

MAJOR 


0.1 SEC 

0.1 

SEC 

J2Z 


334.17 SLUG-FT-FT 452. 

95 KG-M-M 

THRUST 


3 LB 

13.345 NT 

LX 


32 IN 

0.787 M 

LY 


31 IN 

0.762 M 

ACC 


0.0773 FT/SEC/SEC 0.02356 M/SEC/SEC 

wz 


0.04788 RAD/SEC 0.04788 RAD/SEC 

« Q ACH 

a«H H7 m*m h..H 4* m \ 


c 

c 


LG * 4 

OPEN (LG, FILE 


'INITGM.DAT' , STATUS* ' OLD' ) 


C 

READ (LG, 10) MASS 
READ (LG, 10) MAJOR 
READ (LG, 10) J22 

READ (LG, 10) THRUST 
READ (LG, 10) LX 
READ (LG, 10) LY 
READ (LG, 10) EPSL 
READ (LG, 10) EPSA 
READ (LG, 10) UL 
READ (LG, 10) UA 
C 

READ (LG, 20) NTHRX 
READ (LG, 20) NTHRY 
READ (LG, 20) DOF 
C 

READ (LG, 10) SCLX 
READ (LG, 10) SCLY 
C 

DO 100 K - 1, 3 

READ (LG, 10) SNRCCK), SNRB(K) 

100 CONTINUE 
C 

DO 110 K - 1, 3 

READ (LG, 10) NAVCAL(K), NAVOFF(K) 
110 CONTINUE 

C 

DO 120 K - 1, 3 

READ (LG, 10) MTRPCL(K) , MTRPOF ( K ) 
120 CONTINUE 

C 

DO 130 K « 1, 3 

READ (LG, 30) MTRVCL(K), MTRVOF(K) 
130 CONTINUE 

C 

DO 140 K * 1, 3 

READ (LG, 30) DACCAL(K) , DACCOF(K) 
140 CONTINUE 

C 

DO 200 K « 1, DOF 

READ (LG, 10) PGSTN(K) 

OPOSTN(K) - POSTN(K) 

VLCTY(K) - 0.0 
OLDVEL(K) * 0.0 
IF (K .LE. 3) DV( K) * 0.0 
200 CONTINUE 

C 

NN * NTHRX * 4 
DO 300 K ■ 1, NN 

READ (LG, 20) FRTBLXUC, 1 ) 

300 CONTINUE 

C 

NN * NTHRY * 4 
DO 350 K * 1, NN 

READ (LG, 20) FRTBLY ( K , 1 ) 

350 CONTINUE 

C 

C Compute other quantities 

C 

PI * 355.0 / 113.0 

«rn*r> - 1 on n / n r 
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A « THRUST / MASS 



ACC ( 1 ) * 2 * 

NTHRX * 

A 


ACC ( 2 ) * 2 * 

NTHRY * 

A 


CALL SETUP 




RETURN 



10 

FORMAT 

(F15.Q) 


20 

FORMAT 

(12) 


30 

FORMAT 

(F15.8) 



END 

SUBROUTINE WAIT 


This procedure synchronizes T0M_B EXECUTIVE to the interrupt 
service routine. 

This procedure 

A. Transmits the current position & orientation to the 
main-framd computer & 

B. waits until interrupt service routine is completed 
when FLAG is cleared. 

Note that 

FLAG * 0 means system is OK. TOM_B EXECUTIVE sh- 
ould proceed in the normal manner. 

FLAG * -1 means there is a hardware failure of 

some sort. In this case, the main frame 
is notified and the mission aborted. 

FLAG * 1 means not ready. Wait some more. 

There is no provision to halt and power down T0M_B in 
case of hardware failure from software at this time. 


REAL LX, LY 
INTEGER * 4 
INTEGER 
COMMON /CMMD/ 
COMMON /DYNA/ 


FLAG, CMDMOD , CMDRAW<9>, CMDRETC9) 

DOF 

CMDRET, CMDRAW, CMDVAL(9) , CMDMOD, FLAG 
THRUST, ACC ( 2 ) , LX, LY, DOF 


*** Report position *** 
CALL XMIT 


*** Wait until ready *** 

WHILE (FLAG .GT. 0) DO 

IF (FLAG .LE. 0) GO TO 200 
GO TO 100 
END WHILE 


*** See if there is any hardware failures *** 
IF (FLAG .GE. 0) RETURN 


*** We have hardware failure *** 

DO 300 K*1,D0F 
CMDRET(K) » -99 
CONTINUE 

*** Tell mainframe & abort mission 
CALL SENDIT 

c'rnD 


*** 



END1F 


C 
C 
C 

900 RETURN 
END 
C 
C 


C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 


SUBROUTINE XMIT 


This procedure takes the current T0M_B position & places it 
in a buffer. An I / 0 driver SENDIT is called to transmit 
this information to the main frame. 

All lengths are expressed in meters, while all angular quantity 
are expressed in radians. All must be scaled before sending. 


REAL 
INTEGER 
INTEGER * 4 
COMMON /PHYS/ 
COMMON /DYNA/ 
COMMON /PRCN/ 
COMMON /POSN/ 
COMMON /CMMD/ 


LX, LY 
DOF 

CMDRET ( 9 ) , FLAG, CMDMOD , CMDRAW<9) 

MASS, MAJOR, J22, PIRAD 
THRUST, ACC ( 2 ) , LX, LY, DOF 
EPSL, EPSA, UL, UA 
POSTN ( 9 ) , OPOSTN( 9 ) 

CMDRET, CMDRAW, CMDVAL ( 9 ) . CMDMOD, FLAG 




COMMON /RATE/ VLCTY(9), OLDVEL ( 9 ) 

C******* *** ******** *********** ** * *********** ********* AAA Aik AAA ***** 


c 


DO 100 K«1,D0F 
FACTOR » UA 

IF UK .GT. 1) .AND. (K .LT. 5)) FACTOR = UL 
TMP » POSTN(K) * FACTOR 
CMDRET(K) * IFIX ( TMP + 0.5) 

100 CONTINUE 
C 

CALL SENDIT 
C 

RETURN 

END 

C 

C 

SUBROUTINE PMAJOR 


C 

C 

C 

C 

c 

c 

c 

c 

c 


This procedure processes a major cycle by: 

A. determine its current position. 

B. determine its current velocity. 

C. decode the command sequence. 

D. decide if it needs to adjust its position/ velcity 
based on the value of FIRFLG : 


C 

C 

c 

c 

c 

c 

c 

c 

c 


1 : FIRFLG * 0 ; no adjustment needed. 

2 : FIRFLG * 10 ; use thrusters 

3 : FIRFLG * 1 ; use motors 

4 : FIRGLG * 11 ; use both thrusters & motors 

E. In case when both thrusters & motors need to be 
used, the thrusters are fired first. 


FIRFLG, JSTF1 
rvrr r 


INTEGER 

t iffrrro * 
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CUMMUN /ITYLX/ CYCLtt, dSTKi 

**a interpret command sequence & place them in CMDVAH1..6) 
CALL CMDFIX 

*** determine present position & rate *** 

CALL UPDATE 

aaa check to see if it is necessary to move anything aaa 

FIRFLG * 0 

CALL DECI5N (FIRFLG) 

IF (FIRFLG * GE. 10) CALL THRSTR 
JSTF1 * 0 

aaa see if it is necessary to move any motors as well aaa 

FIRFLG * FIRFLG - 10 
IF (FIRFLG .GT. 0) CALL MOTORS 

aaa Grand exit stage left aaa 

RETURN 

END 

SUBROUTINE UPDATE 


This procedure updates the position and velocities of all 
the six axis of the mobile base, after having saved its 
current state 

The axes assignment is as follows : 


Axis 

x xx m 

1 

2 

3 

4 

5 

6 


Dynamic quantity 


yaw of mobile base 
X 
Y 
Z 

pitch 

roll 


Release notes : 

o Triangulation navigation system is not ready. Position 
X and Y are calculated in NAVGN instead of measured. 

o Motor rate feedback is unreliable, but position feedback 
is. Thus, motor rates are derived from the position feed- 

vack data by differentiation, until hardware is rectified. 


INTEGER * 2 
INTEGER * 2 
INTEGER 
REAL 
REAL 

T 


MTRBUF < 6 ) , MTVBUF ( 6 ) 

SNRBUF(3>, NAVBUF ( 3 ) , GYRBUFt 18) , DACBUF ( 6 ) 
DOF 

MASS, MAJOR, J2Z, LX, LY 
MTRPRD( 6 i , MTRPCL ( 6 ) , MTRP0F(6) 

tsrrmr mn r K \ ui'T'DT t f \ \ urmt > c \ 
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REAL " THETA, V(3), JG, W(2), WC3) 

c • • 

COMMON /DYNA/ THRUST, ACCC2), LX, LY. DOF 

COMMON VPHYS/ MASS, MAJOR, JZZ, PIRAD 
COMMON /POSN/ POSTN ( 9 ) , OPOSTN(9) 

COMMON /RATE/ VLCTY(9) , OLDVEL(9) 

COMMON /MOTR/ MTRPRD, MTRPCL, MTRPOF 

COMMON /MOTV/ MTRVRD. MTRVCL, MTRVOF 

COMMON /BUFF/ GYRBUF, NAVBUF, MTRBUF, MTVBUF, SNRBUF, DACBUF 
COMMON /SNSR/ SNRR(3), SNRC(3>, SNRB(3) 

C 

C 

DO 100 K * 1 , DOF 

OPOSTN(K) * POSTN(K) 

OLDVEL(K) * VLCTY(K) 

100 CONTINUE 

THETA * POSTN ( 1 ) 

C 

W(l) * VLCTYC2) 

W( 2 ) * VLCTY ( 3 ) 

CALL FTB (W, THETA, W) 

V(l) * VLCTY ( 1 ) 

V( 2) * W(l) 

V( 3 ) * W( 2 ) 

DO 200 K * 1, 3 
KK * ( K-l ) * 6 
JG * GYRBUF ( KK+1 ) 

DO 220 J * 2, 6 

JG * JG + GYRBUF < KK+J ) 


220 

CONTINUE 



SNRBUF(K) 

* JG / 100000.0 


V(K) 

* V(K) + JG/ 100000 . 0 

200 

CONTINUE 



C 

C transform to floor coordinates 
C 

VLCTY ( 1 ) * V(l) 

V(l) * V(2) 

V( 2 ) » V( 3 ) 

CALL BTF (V, THETA, W) 

VLCTY ( 2) * W(l) 

VLCTY ( 3 > = W{ 2 ) 

C 

CALL NAVGN (MAJOR, GYRBUF, 10) 

C 

C 

C *** Find position & velocity of motors (axes 4.. 6) 

C rates are obtained by differentiation 

C 

KK * DOF - 3 

C IF (KK .LE. 0) GO TO 900 
DO 400 K * 1, KK 

MTRPRD ( K ) * MTRBUF(K) * MTRPCL ( K ) + MTRPOF(K) 
JJ * K + 3 

POSTN ( JJ) * MTRPRD ( K ) 

VLCTY(JJ) x ( POSTN ( JJ) - OPOSTN(JJ)) / MAJOR 
400 CONTINUE 

900 CONTINUE 
C 

RETURN 

END 

C 

C 

SUBROUTINE FTB (F, THETA, B) 
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This subroutine takes a vector F(2) as expressed in flat floor 
coordinates and transforms it to body coordinates through a 
rotation of THETA radians. The transformed vector is placed 
in the array B. 


REAL 

F( 2 ) , B( 2 ) 


c 

COS (THETA) 


s 

SIN (THETA) 


B(l) = 

F( 1 ) a c + 

F( 2 ) * S 

B( 2 ) = 

-F( 1 ) * S + 

F( 2 ) * C 

RETURN 



END 



SUBROUTINE BTF (B, 

THETA, F) 


This subroutine takes a body vector and transforms it to 
flat floor coordinates via a pure rotation by THETA radians. 


REAL 

B( 2 ) , F( 2 ) 



C 

COS (THETA) 



S 

SIN (THETA) 



F( 1 ) * 

B ( 1 ) * C - 

B( 2 ) 

A S 

F( 2 ) « 
RETURN 
END 

B( 1) a S + 

B( 2 ) 

A C 


C 

C 

SUBROUTINE NAVGN (PERIOD, JBUF, N) 

C 

C 

C 

C This is a temporary procedure to determine absolute position & 

C orientation of TOM_B by using the rate information to allow for 

C system checkout. 

C 

C This effectively by-passes the triangulation navigation system. 


C 

c 

c 

c 

c 


c 

n 


This procedure must be replaced, ultimately by an appropriate on 


INTEGER * 2 
REAL * 8 
COMMON /POSN/ 
COMMON /RATE/ 


JBUF(N) 

THETA, B0DE6 
POSTN ( 9 ) , OPOSTNt 9 ) 
VLCTY ( 9 ) , OLDVEL( 9) 


poc - o nnm 
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THETA * B0DE6 ( JBUF , 6, 0.0, 0.1) 

POSTN(l) * THETA 

DO 100 K * 1, 3 

DELTA * (VLCTY(K) ♦ OLDVEL(K) ) * PERIOD / 2.0 
POSTN(K) * 0P0STN(K) * DELTA 
CONTINUE 

RETURN 

END 


REAL * 8 FUNCTION B0DE6 (F, N, A, B) 


This subroutine uses Simpson's rule to perform a simple 
integration to obtain THETA 


INTEGER F(N) 

REAL * 8 H, SUM 

WRITE (*,39) 

FORMAT ( ' in BODE* ) 

H * (B - A) / FLOAT ( N - 1) 

SUM « 19.0 * (FL0AT(F( 1 ) ) + FL0AT(F( 6 ) ) ) 

1 + 75.0 * ( FLOAT ( F ( 2 ) ) + FLOAT ( F ( 5 ) ) ) 

2 ♦ 50.0 * (FL0AT(F( 3 ) ) + FLOAT ( F ( 4 ) ) ) 
B0DE6 * 5.0 * H * SUM / 288.0 

RETURN 

END 


SUBROUTINE CMDFIX 


This procedure processes transmitted commands in CMDRAW and 
calculate their actual values and places them in CMDVAL. 

It is assumed that absolute ( and not delta ) commands will 
be used. Depending on the value of CMDMOD, rate or position 
commands are implemented: 

CMDMOD * 0 means rate control 
CMDMOD * 1 means positional control 

System of units used in T0M_B EXECUTIVE is MKS. 


According to TOM BRYAN, delta commands will never be used, 
but this procedure can be modified if & when delta commands 
are desired. 

Command index assignment: 

INDEX AXIS TYPE MODE=0 M0DE=1 

1 VM4 a 1 1 » v 'T*_rc*r»* X T’WF'T & /VftMt 
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l 

3 

4 

5 

6 


X 

length 

X* 

X 

Y 

length 

Y* 

Y 

Z 

length 

Z* 

A 

PITCH 

angular 

P* 

P 

ROLL 

angular 

R* 

R 

2 FLAG, 

CMDMOD, 

CMDRAW ( 9 ) , 

CMDRET ( 9 ) 


INTEGER 

REAL 

COMMON /PHYS/ 


DOF 


PIRAD 


LX, LY, MASS, MAJOR, JZZ , 

MASS, MAJOR, JZZ, PIRAD 
COMMON /DYNA/ THRUST, ACC(2), LX, LY, DOF 
COMMON /CMMD/ CMDRET, CMDRAW, CMDVAL(9), CMDMOD , FLAG 
COMMON / PRCN / EPSL, EPSA, UL, UA 


*** CONVERT AHOY! *** 

DO 100 K*1 ,D0F 
FACTOR » UA 

IF ((K .GT. 1) .AND. (K .LT. 5)) FACTOR * UL 
RDG * FLOAT ( CMDRAW ( K ) ) / FACTOR 
CMDVAL(K) - RDG 
CONTINUE 


CMDMOD « CMDRAW ( 7 ) 


RETURN 

END 


SUBROUTINE DECISN ( FIRFLG ) 


This procedure decides whether or not corrective action 
needs to be taken by setting and returning a flag FIRFLG : 

A. FIRFLG * 0 ; No action needed 

B. 0<FIRFLG<10 ; Need to move DC motors 

C. FIRFLG >*10 ? Need to fire thrusters 

D. FIRFLG * 11 ; Need to do both 

Decision is made based on the comparison between the com- 
mand sequence & current TOM_B dynamic quantities, remem- 
bering that the system at this instance is under either 
position or rate control, and that the commands are absol- 
ute commands. 


INTEGER DOF, FIRFLG, FG 

INTEGER * 4 FLAG, CMDMOD, CMDRAW ( 9 ) , CMDRET ( 9 ) 

REAL LX, LY 

COMMON /DYNA/ THRUST, ACCC2), LX, LY, DOF 
COMMON /PRCN/ EPSL, EPSA, UL, UA 

COMMON /CMMD/ CMDRET, CMDRAW, CMDVAU9), CMDMOD, FLAG 

*** Check motor section *** 

CALL CHKCMD( 4 ,D0F ,EPSL ,EPSA,FG) 

FIRFLG * FG 

*** Check thrusters section *** 

r r / nunMnn wr nnTr\ t aa 



ORIGINAL PAGE IS 
OE PGOR QU-M.TiY 


’* EPSL~=~ 1.0E-6 
EPSA * EPSL 
C END IF 
100 CONTINUE 

CALL CHKCMD <1,3, EPSA , EPSL , FG ) 

FIRFLG * FIHFLG + FG * 10 
C 

RETURN 

END 

C 

C 

SUBROUTINE CHKCMD(FIRST,LAST,EP1 ,EP 2 ,FG) 

C 

C 

c 

c This procedure checks the absolute command against the ve- 
C hide's position or velocity to determine if any corrective 
C action needs to be taken. If it does, the flag FG sill be 
C set. FG is either 0 or 1 on return from this subroutine. 

C 

C 

c 

INTEGER FIRST , LAST, FG 

INTEGER a 4 FLAG, CMDMOD , CMDRAW<9), CMDRET ( 9 ) 

COMMON /POSN/ P0STN( 9 ) , OPOSTN(9> 

COMMON /RATE/ VLCTY(9>, 0LDVEL(9> 

COMMON /CMMD/ CMDRET, CMDRAW, CMDVAL(9), CMDMOD, FLAG 
C 

C *** initialize loop parameters *** 

C 

FG * 0 
K * FIRST 
EPSLN « EP1 
C 

C *** check between FIRST & LAST inclusive *** 

C 

C REPEAT 

100 T • ABS CPOSTN(K) ) 

IF (CMDMOD .EQ. 0) T - ABS ( VLCTY ( K ) ) 

X « ABS ( CMDVAL ( K ) > 

IF (ABS(X - T) ,GT. EPSLN) FG « 1 
EPSLN « EP2 
K - K + 1 

IF <<K .LE. LAST) .AND. (FG .EQ. 0)) GOTO 100 
C UNTIL K > LAST OR FG » 1 

C 

200 RETURN 
END 
C 
C 

REAL FUNCTION FSICN(X) 

C 

C 

C 

C This procedure returns the sign of a REAL variable as +1.0 
C or -1.0. 

C 

C 

C 

REAL X 
C 

IF (X - 0.0) 100, 200, 200 

100 FSIGN * -1.0 

RETURN 

rerrM -in 
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RETURN 

END 

SUBROUTINE ITABLE 


This procedure initializes all entries of both firing tables 
to zero. 


INTEGER * 2 FRTBLX( 20,2), FRTBLY( 20 , 2) , JETBUFUO) 

COMMON /JETS/ NTHRX, NTHRY, FRTBLX, FRTBLY , JETBUF, SCLX,SCLY 

*** initialize X- firing table *** 

NX * NTHRX * 4 
NY * NTHRY * 4 

DO 100 K*1 ,NX 

FRTBLX ( K , 2 ) * 0 
CONTINUE 

*** Now take care of Y- firing table *** 

DO 200 K * 1 , NY 

FRTBLY < K , 2 ) * 0 
CONTINUE 
DO 300 K*1 ,40 

JETBUF(K) « 0 
CONTINUE 

RETURN 

END 


SUBROUTINE TABLE ( FI ,F2 , NT, TBL, SCALE, NDIR) 


This procedure sets up the appropriate firing table by : 

A- determining the appropriate ♦ of thrusters to be used 

B. calculate the corresponding firing times. & 

C. load the information in the firing table buffer. 

To ensure stability of the vehicle, FI & F2 must be symmetrized 
(if such a word exists at all). 


REAL T( 2 ) , TIME( 2 ) , LX, LY, MASS, MAJOR, J22 

INTEGER BASE( 2 ) ,N( 2 ) ,D0F 

INTEGER * 2 TBL(20,2) 

COMMON /DYNA/ THRUST, ACC(2), LX, LY, DOF 
COMMON /PHYS/ MASS, MAJOR, JZZ, PIRAD 

*** Calculate firing times & make them symmetric when possible 
Firing times are in seconds 

T(l) * FI / THRUST 
T(2) * F2 / THRUST 



ORIGINAL PAGE 13 
OF POOR QUALITY 


C Sane EPS as in TSTFIR 
C 

EPS * 0.001 * MAJOR 
CALL SYMM(T,EPS ) 

C 

C get base indes & actual firing times *** 

C 

DO 100 K-1,2 

BASE(K) * ( K-l ) * NT + 1 
TM * T(K) 

IF (TM .LX. 0) BASE(K) * BASE ( K ) + 2 * NT 

C *** calculate # of thrusters to be used *** 

C TM * ABS(TM) 

CALL NMTHR(TM,NN,NT) 

N(K) * NN 

C *** NOTE: NN is the # of thrusters to be used *** 

TIME(K) * (TM / FLOAT ( NN ) ) * SCALE / MAJOR 
100 CONTINUE 
C 

C *** Symmetrize TIME(l) & TIME(2> 

C 

CALL SYMM( TIME, EPS) 

C 

C *** fill up the firing table buffer *** 

C 

DO 200 K*l,2 
NN » N(K) 

DO 200 J*1,NN 

INDEX * BASE(K) + J - 1 

JM * IFIX (ABS (TIME(K) ) + 0.5) 

TBL( INDEX,2) « JM 
200 CONTINUE 
C 

RETURN 

END 

C 

C 

SUBROUTINE SYMM(T,EPSLN) 

C 

C 

C 

C This procedure symmetrizes two forces T(l), T(2) acting along 

C the same line, but can be in opposite directions. 

C 

C When the magnitudes of the two forces has an absolute dif- 

C ference less than the required precision EPSLN the two raagni- 

C titudes are made to be identical. 

C 

C This procedure is implemented hopefully to take care of minor 

C truncation errors since all computations are carried out in 

C single precision. 

C 

C - 

C 

REAL T( 2) 

C 

C *** Calculate magnitudes & signs of each force 
C 

T1 « T(l) 

ATI * ABS(Tl) 

SI = FSIGN(Tl) 

C 

T2 * T(2) 

AT 2 * ABS ( T2 ) 

f* _ r'C' TPIT f T-t \ 
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TT * AMINl (ATI ,AT2 ) 

*** Now symmetrize them *** 

IF < ABS ( ATI - AT2) .LE. EPSLN) THEN 

IF ( ABS( AT1-AT2 ) .GT. EPSLN) GO TO 100 
T( 1 ) * SI * TT 
T( 2 ) = S2 * TT 
ENDIF 

RETURN 

END 


SUBROUTINE NMTHR(T,NN ,NT) 


This procedure calculates the optimal number of thrusters to 
be used on each side. 

T : Firing time in major cycles 

NN: # of thrusters to be used 

NT: Total # of thrusters available on 1 side. 

At present, it is decided that an ad hoc limit of 5 major cy- 
cles will be used. 

E.G. If it takes 1 thruster for 6 seconds, 

we will use 2 thrusters for 3 seconds. 

Thus, the # of thrusters on each side that is needed is: 

NN * FIRING TIME/ 5 

Once NN is decided, the new firing times must be readjusted to 
reflect the change. This is done in the calling procedure TA- 
BLE. 

It is necessary that 1 <* NN <* NT 


REAL MASS, MAJOR, JZZ 

COMMON /PHYS/ MASS, MAJOR, JZZ, PIRAD 

TX * ABS(T) 

NN - IFIX (TX / MAJOR +0.5) 

IF (NN .EQ. 0) NN = 1 
IF (NN .GT. NT) NN 3 NT 

RETURN 

END 


SUBROUTINE LOADITC K ,TAB , JB ) 


This procedure takes the contents of a firing table & loads 
them into the JET buffer. This feature is implemented for 
easy future expansion when more thrusters will be added. 


JB ( 40 ) 
TAB ( K , 2 ) 

V 


is the jet buffer 

is the appropriate firing table 

I- TIO 


Here, 
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C 

C 

C 

INTEGER * 2 TAB ( K , 2 ) , JB(1) 

INTEGER TIME 

C 

TIME * TAB ( K , 2 ) 

INDEX * TAB ( K , 1 ) 

JB( INDEX) « TIME 
C 

RETURN 

END 

C 

C 

SUBROUTINE FIRE 
C 

C 

C 

C This procedure loads firing times from firing tables into JETSU’ 

C and then invoices the I/O driver LDCTR to fire the appropriate 

C thrusters. NOTE: LDCTR will only load the non-zero table ent- 

C ries. 

C 

C 

c 

INTEGER * 2 FRTBLXC 20 , 2 ) , FRTBLY (20,2) , JETBUF(40), IT, II 
COMMON /JETS/ NTHRX, NTHRY, FRTBLX, FRTBLY, JETBUF , SCLX, SCLY 
C 

C aaa Find the larger of the two aaa 

C 

NX » NTHRX a 4 
NY » NTHRY * 4 
C 

CALL LDBUF (NX, FRTBLX, JETBUF) 

CALL LDBUF (NY, FRTBLY, JETBUF) 

C 

CALL LDCTR( JETBUF, 40) 

C 

RETURN 

END 

C 

C 

SUBROUTINE LDBUF (N, T, J) 

C 

c 

c 

c 

C This subroutine takes the contents of a firing table and per- 

C forms a “this is a good place for a stick up“ and places the 

C corresponding firing times into JETBUF 

C 

C — — 

C 

c 

INTEGER a 2 T(20,2), J(40) 

C 

DO 100 K » 1, N 
IT - TOC, 2) 

II - T(K,1) 

J ( II ) * IT 
100 CONTINUE 

RETURN 
END 
C 
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SUBROUTINE MOTORS 


This procedure calculates the required DC motor rates, converts 
them into DAC values & sends them out to the corresponding DAC. 
An I/O driver is then called on to move the motors. 

The logic depends on the command mode ( Rate or positional 
control) . 

It is explicitly assumed that: 

A. The DC motors are rate driven. Therefore, the DAC out- 
puts dictate the rate. 

B. Each DAC is 12 bit and is wired for bi-polar output. 

C. When position commands are used, a DC motor rate based 
on a three-cycle period is used. The choice of 3 is 
arbitrary, and can be adjusted in the final testing. 


REAL 
INTEGER 
INTECER * 4 
INTEGER * 2 
INTEGER * 2 
COMMON /PHYS/ 
COMMON /DYNA/ 
COMMON /cm D/ 
COMMON /POSN/ 
COMMON /RATE/ 
COMMON /DACO/ 
COMMON /BUFF/ 


MASS, MAJOR, JZZ, LX, LY 
DOF, F 

FLAG, CMDMOD , CMDRAW<9), CMDRET( 9 ) 

GYRBUF( 18 ) 

NAVBUF ( 3 ) , MTRBUF ( 6 ) ,MTVBUF(6) ,SNRBUF(3) ,DACBUF(6 

MASS, MAJOR, JZZ, PIRAD 

THRUST. ACCC2), LX, LY, DOF 

CMDRET, CMDRAW, CMDVAL(9), CMDMOD, FLAG 

POSTN ( 9 ) , OPOSTN(9) 

VLCTY ( 9 ) , OLDVELt 9 ) 

DACRDG( 6 ) , DACCAL ( 6 ) , DACC0F( 6 ) 

GYRBUF , NAVBUF, MTRBUF, MTVBUF, SNRBUF, DACBUF 


*** Whew ! *** 


KK * DOF - 3 
DO 100 MOTOR *1 ,KK 
M * MOTOR 
M3 ■ M ♦ 3 
XCMD * CMDVAL ( M3 ) 

*** Estimate required rate based on mode *** 

0 * XCMD 

IF (CMDMOD .NE. 0) 0* (XCMD-POSTN(M3 ) ) / ( 3 . 0*MAJ0R ) 
*** Convert to DAC count *** 


R « Q * DACCAL ( M ) + DACCOF(M) 

IR » IFIX( R + 0.5) 

SR * FSIGN(R) 

*** Make sure there is no sudden change in direction *** 

X » VLCTY (M3) 

IX * IFIXCX * 100 + 0.5) 

IF (CMDMOD .EQ. 0) THEN 

IF (IX .NE. 0) GOTO 200 
X * 0.0 
GOTO 300 

ELSE 

IF (FSIGN(X) * SR . LT. 0) THEN 

IF ( FSIGN(X) * SR .GE. 0) GOTO 300 

*** There is sign reversal. Better stop motor no 

IR « 0 

CD -in 
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c 

c 

c 

c 

c 

300 


c 

c 

c 


100 

c 

c 

c 

c 


c 

c 

c 

c 

c 

c 

c 

c 

c 


c 


c 

c 

c 


c 

c 

c 

c 

c 


ENDIF 

END IF 

*** Make sure DAC count is within limits *** 
JR * IABS(IR) 

IF (JR .GT. 2047) JR * 2047 

RR s JR * SR 

IR 3 IFIXCRR +0.5) 

*** This is a good place to stick up *** 

DACRDG ( M ) * RR 
DACBUF(M) * IR 
CONTINUE 

*** Move the motors *** 

CALL MTRDRV ( DACBUF , KK ) 

RETURN 

END 


SUBROUTINE THRSTR 


This procedure handles thruster logic. 


REAL FF( 2 ) , F(2), A<2), T(3) 

REAL MASS , MAJOR, JZZ, LX, LY 


INTEGER 
INTEGER * 2 
INTEGER * 4 
COMMON /PHY5/ 
COMMON /DYNA/ 
COMMON /RATE/ 
COMMON /POSN/ 
COMMON /JETS/ 
COMMON /CMMD/ 


DOF 

FRTBLXC 20,2) , FRTBLY( 20 ,2) , JETBUF(40) 
FLAG, CMDMOD , CMDRAW<9), CMDRET ( 9 ) 

MASS, MAJOR, JZZ, PIRAD 
THRUST, ACC ( 2 ) , LX, LY, DOF 
VLCTY ( 9 ) , 0LDVEL<9) 

POSTN ( 9 ) , OPOSTNf 9 ) 

NTHRX, NTHRY, FRTBLX , FRTBLY, JETBUF , SCLX, 
CMDRET, CMDRAW, CMDVAL(9), CMDMOD, FLAG 


SCLY 


transform acceleration vector ACC to floor coordinates 


THETA * POSTN ( 1 ) 

CALL BTF (ACC, THETA, A) 


*** calculate required impulses. This is mode dependent *** 
IF (CMDMOD .EQ. 0) THEN 


IF (CMDMOD ,NE. 0) GOTO 100 
FF( 1 ) * MASS * ( CMDVAL ( 2 ) - VLCTY(2)) / 2 

FF( 2 ) 3 MASS * ( CMDVAL ( 3 ) - VLCTY(3)) / 2 

T0RQ * JZZ * ( CMDVAL ( 1 ) - VLCTY ( 1 ) ) / 2 

GO TO 120 

100 ^^CONTINUE 

DO 150 K 3 1, 3 
V = VLCTY ( K ) 

P 3 POSTN ( K ) 

n - r*Mnt 7* r / v > 
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' IF (K .GT. 1) GOTO 130 

AX = 2 * THRUST * LX / JZZ 
AA * AX 

C IF ((C-P) .LT. 0.0) AA - -AX 

GO TO 135 
C ELSE 

130 AA » ACK-1) 

C END IF 

135 CONTINUE 

C WRITE <*,10) V, P, C, AA 

10 FORMAT r ', 4E15 . 8 ) 

T(K) » G(V, P, C, AA) 

150 CONTINUE 

T1 * T(2> 

T2 • T(3) 

TQ * T(l) 

TORQ *0.0 

IF (ABS(TO) .LT. 0.0001) GOTO 200 
TORQ * THRUST * LX * TO 
200 FF( 1 ) * T1 * MASS * A(l) 

FF( 2 ) * T2 * MASS * A<2) 

C END IF 
120 CONTINUE 
C 

C aaa Transform force from floor coordinates to TOM B coords a*, 
C 

CALL FTB (FF, THETA, F) 

FX * F( 1 ) 

FY « F( 2 ) 

C 

C aaa u a e control laws to calculate force along X & Y directions 
C of T0M_B aaa 

C 

CALL CTRLLW(T0RQ, FX,FY,FX1 ,FX2 ,FY1 ,FY2 ) 

C 

C aaa Convert to firing times and put into firing tables aaa 
C 

CALL ITABLE 

CALL TABLE ( FX1 , FX2 , NTKRX , FRTBLX , SCLX , 2) 

CALL TABLE (FY1 ,FY2 ,NTHRY,FRTBLY,SCLY, 3) 

C 

C aaa Fire them thrusters aaa 

C 

CALL FIRE 
C 

RETURN 

END 

C 

c 

SUBROUTINE CTRLLW< TORO , FX , FY , FX1 , FX2 , FY1 , FY2 ) 

C 

C - 

C 

C This procedure calculates FX1, FX2 from FX & FY1, FY2 from FY 
C & TORQ. 

C It also checks that each FX1 , FX2 , FY1 , FY2 does not exceed 
C the maximum developed thrust on TOM B. 

C 

C 

C 

REAL LX, LY 

INTEGER DOF 

INTEGER a 2 FRTBLX ( 20,2) , FRTBLY (20,2) , JETSUF(40) 

COMMON /DYNA/ THRUST, ACC<2), LX, LY, DOF 

/.nUUOkf / TP'TC / IWNUTiV rH'T'Qr V V cnr v 
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c 

S * 1.0 

C IF (FX .LE. FY) THEN 

IF (FX .GT. FY) GOTO 100 

FY1 = FY / 2.0 + TORQ / (2.0 * LY) 

FY2 = FY - FY1 

CALL CHECK(FY1 ,FY2 ,NTHRY .THRUST, TORQ) 

IF ( (FY1.LT.0.0) .AND. (FY2.LT. 0.0) ) CALL SWAPfFYl ,FY2 ,S ) 
DF * (TORQ + SMFY2 - FY1 ) * LY) / (2 * LX) 

FX1 = FX / 2.0 + DF 
FX2 = FX - FX1 

CALL CHECK ( FX1 . FX2 .NTHRX, THRUST, TORQ) 

IF ( (FX1.LT. 0.0) .AND. (FX2.LT. 0.0) ) CALL SWAP ( FX1 , FX2 , S ) 
GOTO 900 

C ELSE 

100 FX1 * FX / 2 . 0 + TORQ / (2 * LX) 

FX2 = FX - FX1 

CALL CHECK ( FX1 , FX2 , NTHRX , THRUST , TORQ) 

IF ( (FX1.LT.0.0) .AND. (FX2.LT.0.0) ) CALL SWAP(FX1 ,FX2 ,S ) 
DTQ * TORQ + S*(FX2 - FX1 ) * LX 
FY1 = FY / 2.0 + DTQ / (2.0 * LY) 

FY2 * FY - FY1 

CALL CHECK ( FY1 , FY2 ,NTHRY, THRUST, TORQ) 

IF ( (FY1.LT.0.0) .AND. (FY2.LT. 0.0) ) CALL SWAP(FY1 ,FY2 ,S > 
END IF 

900 RETURN 
END 


SUBROUTINE SWAP (X,Y,S) 


This subroutine exchanges X and Y 


REAL T 

S * -1.0 
T « X 
X - Y 
Y « T 
RETURN 
END 


SUBROUTINE CHECK (FI , F2, NTHR, THRUST, TORQ) 


This procedure ensures that the thrust required does not ex- 
ceed the maximum thrust that T0M_B can deliver. 


REAL LIMIT 

REAL MASS , 

INTEGER FG 

COMMON /PHYS/ MASS , 


MAJOR , JZZ , PIRAD 
MAJOR , JZZ, PIRAD 


C 


p»i 
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51 * FSIGN(Fl) 

52 * FSIGN(F2) 

SQ » FSIGN(TORQ) 

FI - ABS (FI) 

F2 * ABS (F2) 

FQ » ABS(TORQ) 

IF (FQ .GT. 0.0001) GOTO 92 
FQ * 0.0 

SQ « 1 

C END IF 

92 CONTINUE 

C 

FG » 1 

IF (F2 .GT. FI) FG « 2 
C 

IF (SI * S2> 100, 200, 200 
100 CONTINUE 

C FI & FI are antiparallel 

IF (FI .GT. FM> FI » FM 

IF (F2 .GT. FM) F2 « FM 

GO TO 800 

C ELSE 

C FI & F2 are parallel 

200 CONTINUE 

DF * ABS (FI - F2 ) 

IF ( (DF.GT. 0.0001) .OR. (FI .GT. 0.0001)) GOTO 207 
FI * 0.0 
F2 * 0.0 
GOTO 800 

207 CONTINUE 

BG * AMAX1 (F1,F2> 

IF (BC .GT. FM) BG * FM 

IF (DF .GT. FM) DF * FM 

CR * BG - DF 

IF (CR .LT. 0.0) CR » 0.0 

IF (FG - 1) 210 , 210 , 220 

C FI >* F2 

210 CONTINUE 

FI « BG 
F2 * CR 
GO TO 700 
C ELSE 

C FI < F2 

220 CONTINUE 

FI » CR 
F2 • BG 

C END IF 

C END IF 

700 CONTINUE 

800 CONTINUE 

FI » SI * FI 

F2 * S2 * F2 

RETURN 
END 

C 

C 

REAL FUNCTION G (VO, XO, CMDX, AC) 

C 

C — 

c 

C This procedure calculates the optimum firing time for 

C thrusters in a direction when position control is used. 

C 

C A distinction is made between a firing time <* 1 major cycle 

/"• .«4 »-V. « . \ 1 »»•* 4mm ....I - 
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c 

C If a firing time < 1/20 of a major cycle, (5 MS) it is set 
C to zero, 

C 

C NOTE : all dynamic variables are in floor coordinates ! 1 I 

C and time is expressed in seconds. 

C 

C 

C 

REAL MASS, MAJOR, J22, PI RAD 

COMMON /PHYS/ MASS, MAJOR, J22, PIRAD 
C 
C 

DX = CMDX - X0 
SV * FSIGN(VO) 

SD * FSIGN(DX) 

IF <<X0 .LT. 0.) .AND. (CMDX .LT. 0.)) GOTO 32 
IF (DX .GE. 0) GOTO 31 
XXO * CMDX 

CMD * X0 

GOTO 38 
C ELSE 

31 XXO * X0 
CMD * CMDX 
GOTO 38 

32 CONTINUE 

IF (DX .GE. 0.0) GOTO 33 
XXO » X0 
CMD « CMDX 
GOTO 38 

33 CONTINUE 

XXO » CMDX 
CMD « XO 
C END IF 

38 CONTINUE 

C 

D * ABS (DX) 

V » ABS (VO) 

A * ABS (AC) 

C 

IF (SD * SV .GT. 0.0) GO TO 50 
C 

C DX and VO are anti-parallel 

C 

T1 * V / A 

RA«T1*T1 + 2*D/A 
T2 * SORT (RA) 

G = SD * (T1 + T2) 

RETURN 

C 

C DX and VO are parallel 

C 

50 CONTINUE 

T » MAJOR 

X * ABS (XXO) 

XI * ABS (XXO) + V * T 
X2 * XI + A * T * T / 2.0 
XC * ABS (CMD) 

C 

C DO CASE 

C Is XC <» XI 

IF (XC .GT. XI) GO TO 200 
RA=T*T- 2.0 a (XI - XC) / A 
IF (RA .LT. 0.0) GO TO 250 

^ - .Pn a / m ennm > n « t \ 


c 

250 


C 
C 

C 2: X2 >« XC > XI 

200 CONTINUE 

IF (XC .GT. X2) GO TO 300 
RA=T*T+2*(X1- XC) / A 
IF (RA .LT. 0.0) GO TO 300 
TF * T - SQRT(RA) 

G * SD * TF 
RETURN 

C END IF 

C 

C 3: XC > X2 

300 CONTINUE 

TF - ( SQRT(V * V + 2.0 * A * D) - V) /A 
G * SD * TF 
RETURN 
C 

C END CASE 

C 

END 

C 

$ 


RETURN 

ELSE 

RA » V * V - 2 
G ■ -SD * (V ^ 
RETURN 
END IF 


* A * (XC • 
SQRT(RA) ) 


X) 
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